Merge pull request #40 from iory/device

Enable device arguments
iory · Feb 27, 2024 · 9979b7f · 9979b7f
2 parents cb64f13 + 4cf2fa3
commit 9979b7f
Show file tree

Hide file tree

Showing 4 changed files with 10 additions and 2 deletions.
diff --git a/rcb4/armh7interface.py b/rcb4/armh7interface.py
@@ -180,7 +180,7 @@ def from_port(port=None):
         if port is not None:
             for cand in ports:
                 if cand.device == port:
-                    if port.vid == 0x165C and port.pid == 0x0008:
+                    if cand.vid == 0x165C and cand.pid == 0x0008:
                         interface = RCB4Interface()
                         interface.open(port)
                         return interface

diff --git a/ros/kxr_controller/launch/kxr_controller.launch b/ros/kxr_controller/launch/kxr_controller.launch
@@ -7,6 +7,7 @@
   <arg name="imu_frame_id" default="/bodyset94472077639384" />
   <arg name="control_loop_rate" default="20" />
   <arg name="use_rcb4" default="false" doc="Flag to use RCB4 mini board"/>
+  <arg name="device" default="" doc="Device path"/>
 
   <group if="$(eval len(arg('namespace')) > 0)" ns="$(arg namespace)" >
     <param name="control_loop_rate" value="$(arg control_loop_rate)" />
@@ -15,6 +16,7 @@
           clear_params="true"
           output="screen"
           required="true" >
+      <param name="device" type="str" value="$(arg device)" />
       <rosparam subst_value="true" >
         urdf_path: $(arg urdf_path)
         servo_config_path: $(arg servo_config_path)
@@ -33,6 +35,7 @@
           clear_params="true"
           output="screen"
           required="true" >
+      <param name="device" type="str" value="$(arg device)" />
       <rosparam subst_value="true" >
         urdf_path: $(arg urdf_path)
         servo_config_path: $(arg servo_config_path)

diff --git a/ros/kxr_controller/launch/kxr_controller_for_wheel.launch b/ros/kxr_controller/launch/kxr_controller_for_wheel.launch
@@ -3,11 +3,13 @@
   <arg name="urdf_path" default="$(find kxr_models)/urdf/kxrmw4a6h2m.urdf" />
   <arg name="servo_config_path" default="$(find kxr_models)/config/kxrmw4a6h2m_servo_config.yaml" />
   <arg name="use_rcb4" default="false" doc="Flag to use RCB4 mini board"/>
+  <arg name="device" default="" doc="Device path"/>
 
   <include file="$(find kxr_controller)/launch/kxr_controller.launch" >
     <arg name="urdf_path" value="$(arg urdf_path)" />
     <arg name="servo_config_path" value="$(arg servo_config_path)" />
     <arg name="use_rcb4" value="$(arg use_rcb4)" />
+    <arg name="device" value="$(arg device)" />
   </include>
 
   <include file="$(find kxr_controller)/launch/diff_drive_controller.launch" />

diff --git a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
@@ -130,7 +130,10 @@ def __init__(self):
             JointState,
             queue_size=1)
 
-        if rospy.get_param('~use_rcb4'):
+        if rospy.get_param('~device', None):
+            self.interface = ARMH7Interface.from_port(
+                rospy.get_param('~device', None))
+        elif rospy.get_param('~use_rcb4'):
             self.interface = RCB4Interface()
             self.interface.auto_open()
         else: