diff --git a/scripts/linker_hand_control.sh b/scripts/linker_hand_control.sh index 1a3f44a..0e55d2f 100755 --- a/scripts/linker_hand_control.sh +++ b/scripts/linker_hand_control.sh @@ -10,6 +10,11 @@ LOG_FILE="${WS_DIR}/log/linker_hand_control.log" CAN_INTERFACES=(can0 can1) CAN_BITRATE=1000000 +# Keep this driver isolated from ROS 2 nodes on other machines in the same LAN. +# Without this, DDS discovery can pick up stale linker_hand_sdk nodes from the +# subnet and make the same topic appear to have multiple publishers. +export ROS_AUTOMATIC_DISCOVERY_RANGE=LOCALHOST + usage() { echo "Usage: $0 {start|stop|status}" } @@ -45,6 +50,11 @@ source_workspace() { set -u } +restart_ros_daemon_localhost() { + ros2 daemon stop >/dev/null 2>&1 || true + ros2 daemon start >/dev/null 2>&1 || true +} + start_can() { for can_if in "${CAN_INTERFACES[@]}"; do if ! ip link show "${can_if}" >/dev/null 2>&1; then @@ -77,13 +87,14 @@ start_driver() { fi source_workspace + restart_ros_daemon_localhost start_can echo "Starting linker_hand service..." echo "Log: ${LOG_FILE}" : > "${LOG_FILE}" - setsid bash -lc "set +u; source '${WS_DIR}/install/setup.bash'; set -u; exec ros2 launch linker_hand_ros2_sdk linker_hand_double.launch.py" \ + setsid bash -lc "export ROS_AUTOMATIC_DISCOVERY_RANGE=LOCALHOST; set +u; source '${WS_DIR}/install/setup.bash'; set -u; exec ros2 launch linker_hand_ros2_sdk linker_hand_double.launch.py" \ >> "${LOG_FILE}" 2>&1 & local pid=$! @@ -151,6 +162,8 @@ status_driver() { echo "linker_hand service is not running" fi + echo "ROS_AUTOMATIC_DISCOVERY_RANGE=${ROS_AUTOMATIC_DISCOVERY_RANGE}" + for can_if in "${CAN_INTERFACES[@]}"; do if ip link show "${can_if}" >/dev/null 2>&1; then ip -details link show "${can_if}" | sed -n '1,2p' diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py index 34d4e1d..3f0542d 100644 --- a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py @@ -151,7 +151,7 @@ class LinkerHand(Node): self.hz = 1.0/60.0 self.hand_setting_sub = self.create_subscription( - String, self.topic('/cb_hand_setting_cmd'), self.hand_setting_cb, 10 + String, self.topic('/l20_hand_setting_cmd'), self.hand_setting_cb, 10 ) self._init_hand() time.sleep(1) @@ -181,31 +181,31 @@ class LinkerHand(Node): self.touch_type = self.api.get_touch_type() self.hand_cmd_sub = self.create_subscription( JointState, - self.topic(f'/cb_{self.hand_type}_hand_control_cmd'), + self.topic(f'/l20_{self.hand_type}_hand_control_cmd'), self.hand_control_cb, COMMAND_QOS, ) - self.hand_state_pub = self.create_publisher(JointState, self.topic(f'/cb_{self.hand_type}_hand_state'),10) - self.hand_info_pub = self.create_publisher(String, self.topic(f'/cb_{self.hand_type}_hand_info'), 10) + self.hand_state_pub = self.create_publisher(JointState, self.topic(f'/l20_{self.hand_type}_hand_state'),10) + self.hand_info_pub = self.create_publisher(String, self.topic(f'/l20_{self.hand_type}_hand_info'), 10) if self.is_touch == True: if self.modbus != "None": - self.matrix_touch_pub = self.create_publisher(String, self.topic(f'/cb_{self.hand_type}_hand_matrix_touch'), 10) - self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, self.topic(f'/cb_{self.hand_type}_hand_matrix_touch_pc'), 10) - self.matrix_touch_mass_pub = self.create_publisher(String, self.topic(f'/cb_{self.hand_type}_hand_matrix_touch_mass'), 10) + self.matrix_touch_pub = self.create_publisher(String, self.topic(f'/l20_{self.hand_type}_hand_matrix_touch'), 10) + self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, self.topic(f'/l20_{self.hand_type}_hand_matrix_touch_pc'), 10) + self.matrix_touch_mass_pub = self.create_publisher(Float32MultiArray, self.topic(f'/l20_{self.hand_type}_hand_matrix_touch_mass'), 10) elif self.hand_joint == "L20": ColorMsg(msg=f"{self.hand_type} {self.hand_joint} matrix pressure sensing enabled", color='green') self.touch_type = 2 - self.matrix_touch_pub = self.create_publisher(String, self.topic(f'/cb_{self.hand_type}_hand_matrix_touch'), 10) - self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, self.topic(f'/cb_{self.hand_type}_hand_matrix_touch_pc'), 10) - self.matrix_touch_mass_pub = self.create_publisher(String, self.topic(f'/cb_{self.hand_type}_hand_matrix_touch_mass'), 10) + self.matrix_touch_pub = self.create_publisher(String, self.topic(f'/l20_{self.hand_type}_hand_matrix_touch'), 10) + self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, self.topic(f'/l20_{self.hand_type}_hand_matrix_touch_pc'), 10) + self.matrix_touch_mass_pub = self.create_publisher(Float32MultiArray, self.topic(f'/l20_{self.hand_type}_hand_matrix_touch_mass'), 10) elif self.touch_type > 1: ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with matrix pressure sensing", color='green') - self.matrix_touch_pub = self.create_publisher(String, self.topic(f'/cb_{self.hand_type}_hand_matrix_touch'), 10) - self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, self.topic(f'/cb_{self.hand_type}_hand_matrix_touch_pc'), 10) - self.matrix_touch_mass_pub = self.create_publisher(String, self.topic(f'/cb_{self.hand_type}_hand_matrix_touch_mass'), 10) + self.matrix_touch_pub = self.create_publisher(String, self.topic(f'/l20_{self.hand_type}_hand_matrix_touch'), 10) + self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, self.topic(f'/l20_{self.hand_type}_hand_matrix_touch_pc'), 10) + self.matrix_touch_mass_pub = self.create_publisher(Float32MultiArray, self.topic(f'/l20_{self.hand_type}_hand_matrix_touch_mass'), 10) elif self.touch_type != -1 and self.modbus == "None": ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with pressure sensor", color="green") - self.touch_pub = self.create_publisher(Float32MultiArray, self.topic(f'/cb_{self.hand_type}_hand_force'), 10) + self.touch_pub = self.create_publisher(Float32MultiArray, self.topic(f'/l20_{self.hand_type}_hand_force'), 10) else: ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Not equipped with any pressure sensors", color="red") self.is_touch = False @@ -410,7 +410,7 @@ class LinkerHand(Node): if self.is_touch == True and (self.touch_type > 1 or self.modbus != "None") and (self.matrix_touch_pub.get_subscription_count() > 0 or self.matrix_touch_mass_pub.get_subscription_count() > 0 or self.matrix_touch_pub_pc.get_subscription_count() > 0): # 发布矩阵压感数据JSON格式 self.pub_matrix_dic() - # 发布矩阵压感和值JSON格式 + # 发布矩阵压感合值浮点数组 self.pub_matrix_mass(dic=self.matrix_dic) # 发布矩阵压感点云格式 self.pub_matrix_point_cloud() @@ -421,22 +421,15 @@ class LinkerHand(Node): time.sleep(self.hz) def pub_matrix_mass(self, dic): - """发布矩阵数据合值 单位g 克 JSON格式""" - msg = String() - # 获取当前的 ROS 时间 - current_time = self.stamp_clock.now() - # 提取 secs 和 nsecs - t_secs = current_time.to_msg().sec - t_nsecs = current_time.to_msg().nanosec - self.matrix_mass_dic["stamp"]["secs"] = t_secs - self.matrix_mass_dic["stamp"]["nsecs"] = t_nsecs - self.matrix_mass_dic["unit"] = "g" - self.matrix_mass_dic["thumb_mass"] = sum(sum(row) for row in dic["thumb_matrix"]) - self.matrix_mass_dic["index_mass"] = sum(sum(row) for row in dic["index_matrix"]) - self.matrix_mass_dic["middle_mass"] = sum(sum(row) for row in dic["middle_matrix"]) - self.matrix_mass_dic["ring_mass"] = sum(sum(row) for row in dic["ring_matrix"]) - self.matrix_mass_dic["little_mass"] = sum(sum(row) for row in dic["little_matrix"]) - msg.data = json.dumps(self.matrix_mass_dic) + """发布矩阵数据合值,顺序为 thumb, index, middle, ring, little,单位 g。""" + msg = Float32MultiArray() + msg.data = [ + float(sum(sum(row) for row in dic["thumb_matrix"])), + float(sum(sum(row) for row in dic["index_matrix"])), + float(sum(sum(row) for row in dic["middle_matrix"])), + float(sum(sum(row) for row in dic["ring_matrix"])), + float(sum(sum(row) for row in dic["little_matrix"])), + ] self.matrix_touch_mass_pub.publish(msg) def pub_matrix_point_cloud(self): diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_g20.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_g20.py index 2bf1f83..f4b7d75 100644 --- a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_g20.py +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_g20.py @@ -87,17 +87,17 @@ class LinkerHandAdvancedG20(Node): self.api = LinkerHandApi(hand_type=self.hand_type, hand_joint=self.hand_joint,modbus=self.modbus,can=self.can) time.sleep(0.1) self.touch_type = self.api.get_touch_type() - self.hand_cmd_sub = self.create_subscription(JointState, f'/cb_{self.hand_type}_hand_control_cmd', self.hand_control_cb,10) - self.hand_state_pub = self.create_publisher(JointState, f'/cb_{self.hand_type}_hand_state',10) + self.hand_cmd_sub = self.create_subscription(JointState, f'/l20_{self.hand_type}_hand_control_cmd', self.hand_control_cb,10) + self.hand_state_pub = self.create_publisher(JointState, f'/l20_{self.hand_type}_hand_state',10) if self.is_touch == True: if self.touch_type > 1: ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with matrix pressure sensing", color='green') - self.matrix_touch_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch', 10) - self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/cb_{self.hand_type}_hand_matrix_touch_pc', 10) - self.matrix_touch_mass_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch_mass', 10) + self.matrix_touch_pub = self.create_publisher(String, f'/l20_{self.hand_type}_hand_matrix_touch', 10) + self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/l20_{self.hand_type}_hand_matrix_touch_pc', 10) + self.matrix_touch_mass_pub = self.create_publisher(Float32MultiArray, f'/l20_{self.hand_type}_hand_matrix_touch_mass', 10) elif self.touch_type != -1: ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with pressure sensor", color="green") - self.touch_pub = self.create_publisher(Float32MultiArray, f'/cb_{self.hand_type}_hand_force', 10) + self.touch_pub = self.create_publisher(Float32MultiArray, f'/l20_{self.hand_type}_hand_force', 10) else: ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Not equipped with any pressure sensors", color="red") self.is_touch = False @@ -163,7 +163,7 @@ class LinkerHandAdvancedG20(Node): self.matrix_dic["little_matrix"] = self.api.get_little_matrix_touch(sleep_time=TOUCH_SLEEP_TIME).tolist() # 发布矩阵压感数据JSON格式 self.pub_matrix_dic() - # 发布矩阵压感和值JSON格式 + # 发布矩阵压感合值浮点数组 self.pub_matrix_mass(dic=self.matrix_dic) # 发布矩阵压感点云格式 self.pub_matrix_point_cloud() @@ -185,22 +185,15 @@ class LinkerHandAdvancedG20(Node): self.matrix_touch_pub.publish(msg) def pub_matrix_mass(self, dic): - """发布矩阵数据合值 单位g 克 JSON格式""" - msg = String() - # 获取当前的 ROS 时间 - current_time = self.stamp_clock.now() - # 提取 secs 和 nsecs - t_secs = current_time.to_msg().sec - t_nsecs = current_time.to_msg().nanosec - self.matrix_mass_dic["stamp"]["secs"] = t_secs - self.matrix_mass_dic["stamp"]["nsecs"] = t_nsecs - self.matrix_mass_dic["unit"] = "g" - self.matrix_mass_dic["thumb_mass"] = sum(sum(row) for row in dic["thumb_matrix"]) - self.matrix_mass_dic["index_mass"] = sum(sum(row) for row in dic["index_matrix"]) - self.matrix_mass_dic["middle_mass"] = sum(sum(row) for row in dic["middle_matrix"]) - self.matrix_mass_dic["ring_mass"] = sum(sum(row) for row in dic["ring_matrix"]) - self.matrix_mass_dic["little_mass"] = sum(sum(row) for row in dic["little_matrix"]) - msg.data = json.dumps(self.matrix_mass_dic) + """发布矩阵数据合值,顺序为 thumb, index, middle, ring, little,单位 g。""" + msg = Float32MultiArray() + msg.data = [ + float(sum(sum(row) for row in dic["thumb_matrix"])), + float(sum(sum(row) for row in dic["index_matrix"])), + float(sum(sum(row) for row in dic["middle_matrix"])), + float(sum(sum(row) for row in dic["ring_matrix"])), + float(sum(sum(row) for row in dic["little_matrix"])), + ] self.matrix_touch_mass_pub.publish(msg) def pub_matrix_point_cloud(self): @@ -232,9 +225,9 @@ class LinkerHandAdvancedG20(Node): def main(args=None): ''' 本节点用于收集手指状态和压感数据。 - '/cb_{self.hand_type}_hand_control_cmd' 话题类型为 sensor_msgs/msg/JointState 控制话题,限制 30Hz - /cb_{self.hand_type}_hand_state 话题类型为 sensor_msgs/msg/JointState 30Hz - '/cb_{self.hand_type}_hand_matrix_touch' 话题类型为 std_msgs/msg/String 30Hz + '/l20_{self.hand_type}_hand_control_cmd' 话题类型为 sensor_msgs/msg/JointState 控制话题,限制 30Hz + /l20_{self.hand_type}_hand_state 话题类型为 sensor_msgs/msg/JointState 30Hz + '/l20_{self.hand_type}_hand_matrix_touch' 话题类型为 std_msgs/msg/String 30Hz 启动命令: ros2 run linker_hand_ros2_sdk linker_hand_advanced_g20 --hand_type left --can can0 --is_touch true ''' diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l10.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l10.py index 37a7be3..4026aef 100644 --- a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l10.py +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l10.py @@ -76,17 +76,17 @@ class LinkerHandAdvancedL10(Node): self.api = LinkerHandApi(hand_type=self.hand_type, hand_joint=self.hand_joint,modbus=self.modbus,can=self.can) time.sleep(0.1) self.touch_type = self.api.get_touch_type() - self.hand_cmd_sub = self.create_subscription(JointState, f'/cb_{self.hand_type}_hand_control_cmd', self.hand_control_cb,10) - self.hand_state_pub = self.create_publisher(JointState, f'/cb_{self.hand_type}_hand_state',10) + self.hand_cmd_sub = self.create_subscription(JointState, f'/l20_{self.hand_type}_hand_control_cmd', self.hand_control_cb,10) + self.hand_state_pub = self.create_publisher(JointState, f'/l20_{self.hand_type}_hand_state',10) if self.is_touch == True: if self.touch_type > 1: ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with matrix pressure sensing", color='green') - self.matrix_touch_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch', 10) - self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/cb_{self.hand_type}_hand_matrix_touch_pc', 10) - self.matrix_touch_mass_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch_mass', 10) + self.matrix_touch_pub = self.create_publisher(String, f'/l20_{self.hand_type}_hand_matrix_touch', 10) + self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/l20_{self.hand_type}_hand_matrix_touch_pc', 10) + self.matrix_touch_mass_pub = self.create_publisher(Float32MultiArray, f'/l20_{self.hand_type}_hand_matrix_touch_mass', 10) elif self.touch_type != -1: ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with pressure sensor", color="green") - self.touch_pub = self.create_publisher(Float32MultiArray, f'/cb_{self.hand_type}_hand_force', 10) + self.touch_pub = self.create_publisher(Float32MultiArray, f'/l20_{self.hand_type}_hand_force', 10) else: ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Not equipped with any pressure sensors", color="red") self.is_touch = False @@ -151,7 +151,7 @@ class LinkerHandAdvancedL10(Node): self.matrix_dic["little_matrix"] = self.api.get_little_matrix_touch(sleep_time=0.004).tolist() # 发布矩阵压感数据JSON格式 self.pub_matrix_dic() - # 发布矩阵压感和值JSON格式 + # 发布矩阵压感合值浮点数组 self.pub_matrix_mass(dic=self.matrix_dic) # 发布矩阵压感点云格式 self.pub_matrix_point_cloud() @@ -171,22 +171,15 @@ class LinkerHandAdvancedL10(Node): self.matrix_touch_pub.publish(msg) def pub_matrix_mass(self, dic): - """发布矩阵数据合值 单位g 克 JSON格式""" - msg = String() - # 获取当前的 ROS 时间 - current_time = self.stamp_clock.now() - # 提取 secs 和 nsecs - t_secs = current_time.to_msg().sec - t_nsecs = current_time.to_msg().nanosec - self.matrix_mass_dic["stamp"]["secs"] = t_secs - self.matrix_mass_dic["stamp"]["nsecs"] = t_nsecs - self.matrix_mass_dic["unit"] = "g" - self.matrix_mass_dic["thumb_mass"] = sum(sum(row) for row in dic["thumb_matrix"]) - self.matrix_mass_dic["index_mass"] = sum(sum(row) for row in dic["index_matrix"]) - self.matrix_mass_dic["middle_mass"] = sum(sum(row) for row in dic["middle_matrix"]) - self.matrix_mass_dic["ring_mass"] = sum(sum(row) for row in dic["ring_matrix"]) - self.matrix_mass_dic["little_mass"] = sum(sum(row) for row in dic["little_matrix"]) - msg.data = json.dumps(self.matrix_mass_dic) + """发布矩阵数据合值,顺序为 thumb, index, middle, ring, little,单位 g。""" + msg = Float32MultiArray() + msg.data = [ + float(sum(sum(row) for row in dic["thumb_matrix"])), + float(sum(sum(row) for row in dic["index_matrix"])), + float(sum(sum(row) for row in dic["middle_matrix"])), + float(sum(sum(row) for row in dic["ring_matrix"])), + float(sum(sum(row) for row in dic["little_matrix"])), + ] self.matrix_touch_mass_pub.publish(msg) def pub_matrix_point_cloud(self): @@ -218,11 +211,11 @@ class LinkerHandAdvancedL10(Node): def main(args=None): ''' 本节点用于收集手指状态和压感数据。 - '/cb_{self.hand_type}_hand_control_cmd' 话题类型为 sensor_msgs/msg/JointState 控制话题,限制 30Hz - /cb_{self.hand_type}_hand_state 话题类型为 sensor_msgs/msg/JointState 30Hz - '/cb_{self.hand_type}_hand_matrix_touch' 话题类型为 std_msgs/msg/String 30Hz - '/cb_{self.hand_type}_hand_matrix_touch_pc', 10) - self.matrix_touch_mass_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch_mass', 10) + '/l20_{self.hand_type}_hand_control_cmd' 话题类型为 sensor_msgs/msg/JointState 控制话题,限制 30Hz + /l20_{self.hand_type}_hand_state 话题类型为 sensor_msgs/msg/JointState 30Hz + '/l20_{self.hand_type}_hand_matrix_touch' 话题类型为 std_msgs/msg/String 30Hz + '/l20_{self.hand_type}_hand_matrix_touch_pc', 10) + self.matrix_touch_mass_pub = self.create_publisher(Float32MultiArray, f'/l20_{self.hand_type}_hand_matrix_touch_mass', 10) 启动命令: ros2 run linker_hand_ros2_sdk linker_hand_advanced_l10 --hand_type left --can can0 --is_touch true ''' diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l6.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l6.py index 2672283..81c6222 100644 --- a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l6.py +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l6.py @@ -78,17 +78,17 @@ class LinkerHandAdvancedL6(Node): self.api = LinkerHandApi(hand_type=self.hand_type, hand_joint=self.hand_joint,modbus=self.modbus,can=self.can) time.sleep(0.1) self.touch_type = self.api.get_touch_type() - self.hand_cmd_sub = self.create_subscription(JointState, f'/cb_{self.hand_type}_hand_control_cmd', self.hand_control_cb,10) - self.hand_state_pub = self.create_publisher(JointState, f'/cb_{self.hand_type}_hand_state',10) + self.hand_cmd_sub = self.create_subscription(JointState, f'/l20_{self.hand_type}_hand_control_cmd', self.hand_control_cb,10) + self.hand_state_pub = self.create_publisher(JointState, f'/l20_{self.hand_type}_hand_state',10) if self.is_touch == True: if self.touch_type > 1: ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with matrix pressure sensing", color='green') - self.matrix_touch_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch', 10) - self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/cb_{self.hand_type}_hand_matrix_touch_pc', 10) - self.matrix_touch_mass_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch_mass', 10) + self.matrix_touch_pub = self.create_publisher(String, f'/l20_{self.hand_type}_hand_matrix_touch', 10) + self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/l20_{self.hand_type}_hand_matrix_touch_pc', 10) + self.matrix_touch_mass_pub = self.create_publisher(Float32MultiArray, f'/l20_{self.hand_type}_hand_matrix_touch_mass', 10) elif self.touch_type != -1: ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with pressure sensor", color="green") - self.touch_pub = self.create_publisher(Float32MultiArray, f'/cb_{self.hand_type}_hand_force', 10) + self.touch_pub = self.create_publisher(Float32MultiArray, f'/l20_{self.hand_type}_hand_force', 10) else: ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Not equipped with any pressure sensors", color="red") self.is_touch = False @@ -155,7 +155,7 @@ class LinkerHandAdvancedL6(Node): self.matrix_dic["little_matrix"] = self.api.get_little_matrix_touch(sleep_time=0.003).tolist() # 发布矩阵压感数据JSON格式 self.pub_matrix_dic() - # 发布矩阵压感和值JSON格式 + # 发布矩阵压感合值浮点数组 self.pub_matrix_mass(dic=self.matrix_dic) # 发布矩阵压感点云格式 self.pub_matrix_point_cloud() @@ -174,22 +174,15 @@ class LinkerHandAdvancedL6(Node): self.matrix_touch_pub.publish(msg) def pub_matrix_mass(self, dic): - """发布矩阵数据合值 单位g 克 JSON格式""" - msg = String() - # 获取当前的 ROS 时间 - current_time = self.stamp_clock.now() - # 提取 secs 和 nsecs - t_secs = current_time.to_msg().sec - t_nsecs = current_time.to_msg().nanosec - self.matrix_mass_dic["stamp"]["secs"] = t_secs - self.matrix_mass_dic["stamp"]["nsecs"] = t_nsecs - self.matrix_mass_dic["unit"] = "g" - self.matrix_mass_dic["thumb_mass"] = sum(sum(row) for row in dic["thumb_matrix"]) - self.matrix_mass_dic["index_mass"] = sum(sum(row) for row in dic["index_matrix"]) - self.matrix_mass_dic["middle_mass"] = sum(sum(row) for row in dic["middle_matrix"]) - self.matrix_mass_dic["ring_mass"] = sum(sum(row) for row in dic["ring_matrix"]) - self.matrix_mass_dic["little_mass"] = sum(sum(row) for row in dic["little_matrix"]) - msg.data = json.dumps(self.matrix_mass_dic) + """发布矩阵数据合值,顺序为 thumb, index, middle, ring, little,单位 g。""" + msg = Float32MultiArray() + msg.data = [ + float(sum(sum(row) for row in dic["thumb_matrix"])), + float(sum(sum(row) for row in dic["index_matrix"])), + float(sum(sum(row) for row in dic["middle_matrix"])), + float(sum(sum(row) for row in dic["ring_matrix"])), + float(sum(sum(row) for row in dic["little_matrix"])), + ] self.matrix_touch_mass_pub.publish(msg) def pub_matrix_point_cloud(self): @@ -222,9 +215,9 @@ class LinkerHandAdvancedL6(Node): def main(args=None): ''' 本节点用于收集手指状态和压感数据。 - '/cb_{self.hand_type}_hand_control_cmd' 话题类型为 sensor_msgs/msg/JointState 控制话题,限制 30Hz - /cb_{self.hand_type}_hand_state 话题类型为 sensor_msgs/msg/JointState 40Hz - '/cb_{self.hand_type}_hand_matrix_touch' 话题类型为 std_msgs/msg/String 40Hz + '/l20_{self.hand_type}_hand_control_cmd' 话题类型为 sensor_msgs/msg/JointState 控制话题,限制 30Hz + /l20_{self.hand_type}_hand_state 话题类型为 sensor_msgs/msg/JointState 40Hz + '/l20_{self.hand_type}_hand_matrix_touch' 话题类型为 std_msgs/msg/String 40Hz 启动命令: ros2 run linker_hand_ros2_sdk linker_hand_advanced_l6 --hand_type left --can can0 --is_touch true ''' diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l7.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l7.py index 7ed58bd..37a5da2 100644 --- a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l7.py +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l7.py @@ -79,17 +79,17 @@ class LinkerHandAdvancedL7(Node): self.api = LinkerHandApi(hand_type=self.hand_type, hand_joint=self.hand_joint,modbus=self.modbus,can=self.can) time.sleep(0.1) self.touch_type = self.api.get_touch_type() - self.hand_cmd_sub = self.create_subscription(JointState, f'/cb_{self.hand_type}_hand_control_cmd', self.hand_control_cb,10) - self.hand_state_pub = self.create_publisher(JointState, f'/cb_{self.hand_type}_hand_state',10) + self.hand_cmd_sub = self.create_subscription(JointState, f'/l20_{self.hand_type}_hand_control_cmd', self.hand_control_cb,10) + self.hand_state_pub = self.create_publisher(JointState, f'/l20_{self.hand_type}_hand_state',10) if self.is_touch == True: if self.touch_type > 1: ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with matrix pressure sensing", color='green') - self.matrix_touch_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch', 10) - self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/cb_{self.hand_type}_hand_matrix_touch_pc', 10) - self.matrix_touch_mass_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch_mass', 10) + self.matrix_touch_pub = self.create_publisher(String, f'/l20_{self.hand_type}_hand_matrix_touch', 10) + self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/l20_{self.hand_type}_hand_matrix_touch_pc', 10) + self.matrix_touch_mass_pub = self.create_publisher(Float32MultiArray, f'/l20_{self.hand_type}_hand_matrix_touch_mass', 10) elif self.touch_type != -1: ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with pressure sensor", color="green") - self.touch_pub = self.create_publisher(Float32MultiArray, f'/cb_{self.hand_type}_hand_force', 10) + self.touch_pub = self.create_publisher(Float32MultiArray, f'/l20_{self.hand_type}_hand_force', 10) else: ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Not equipped with any pressure sensors", color="red") self.is_touch = False @@ -185,7 +185,7 @@ class LinkerHandAdvancedL7(Node): self.matrix_dic["little_matrix"] = self.api.get_little_matrix_touch(sleep_time=0.006).tolist() # 发布矩阵压感数据JSON格式 self.pub_matrix_dic() - # 发布矩阵压感和值JSON格式 + # 发布矩阵压感合值浮点数组 self.pub_matrix_mass(dic=self.matrix_dic) # 发布矩阵压感点云格式 self.pub_matrix_point_cloud() @@ -208,22 +208,15 @@ class LinkerHandAdvancedL7(Node): self.matrix_touch_pub.publish(msg) def pub_matrix_mass(self, dic): - """发布矩阵数据合值 单位g 克 JSON格式""" - msg = String() - # 获取当前的 ROS 时间 - current_time = self.stamp_clock.now() - # 提取 secs 和 nsecs - t_secs = current_time.to_msg().sec - t_nsecs = current_time.to_msg().nanosec - self.matrix_mass_dic["stamp"]["secs"] = t_secs - self.matrix_mass_dic["stamp"]["nsecs"] = t_nsecs - self.matrix_mass_dic["unit"] = "g" - self.matrix_mass_dic["thumb_mass"] = sum(sum(row) for row in dic["thumb_matrix"]) - self.matrix_mass_dic["index_mass"] = sum(sum(row) for row in dic["index_matrix"]) - self.matrix_mass_dic["middle_mass"] = sum(sum(row) for row in dic["middle_matrix"]) - self.matrix_mass_dic["ring_mass"] = sum(sum(row) for row in dic["ring_matrix"]) - self.matrix_mass_dic["little_mass"] = sum(sum(row) for row in dic["little_matrix"]) - msg.data = json.dumps(self.matrix_mass_dic) + """发布矩阵数据合值,顺序为 thumb, index, middle, ring, little,单位 g。""" + msg = Float32MultiArray() + msg.data = [ + float(sum(sum(row) for row in dic["thumb_matrix"])), + float(sum(sum(row) for row in dic["index_matrix"])), + float(sum(sum(row) for row in dic["middle_matrix"])), + float(sum(sum(row) for row in dic["ring_matrix"])), + float(sum(sum(row) for row in dic["little_matrix"])), + ] self.matrix_touch_mass_pub.publish(msg) def pub_matrix_point_cloud(self): @@ -255,9 +248,9 @@ class LinkerHandAdvancedL7(Node): def main(args=None): ''' 本节点用于收集手指状态和压感数据。 - '/cb_{self.hand_type}_hand_control_cmd' 话题类型为 sensor_msgs/msg/JointState 控制话题,限制 30Hz - /cb_{self.hand_type}_hand_state 话题类型为 sensor_msgs/msg/JointState 40Hz - '/cb_{self.hand_type}_hand_matrix_touch' 话题类型为 std_msgs/msg/String 40Hz + '/l20_{self.hand_type}_hand_control_cmd' 话题类型为 sensor_msgs/msg/JointState 控制话题,限制 30Hz + /l20_{self.hand_type}_hand_state 话题类型为 sensor_msgs/msg/JointState 40Hz + '/l20_{self.hand_type}_hand_matrix_touch' 话题类型为 std_msgs/msg/String 40Hz 启动命令: ros2 run linker_hand_ros2_sdk linker_hand_advanced_l7 --hand_type left --can can0 --is_touch true ''' diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_o6.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_o6.py index 42638c8..882913a 100644 --- a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_o6.py +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_o6.py @@ -77,17 +77,17 @@ class LinkerHandAdvancedO6(Node): self.api = LinkerHandApi(hand_type=self.hand_type, hand_joint=self.hand_joint,modbus=self.modbus,can=self.can) time.sleep(0.1) self.touch_type = self.api.get_touch_type() - self.hand_cmd_sub = self.create_subscription(JointState, f'/cb_{self.hand_type}_hand_control_cmd', self.hand_control_cb,10) - self.hand_state_pub = self.create_publisher(JointState, f'/cb_{self.hand_type}_hand_state',10) + self.hand_cmd_sub = self.create_subscription(JointState, f'/l20_{self.hand_type}_hand_control_cmd', self.hand_control_cb,10) + self.hand_state_pub = self.create_publisher(JointState, f'/l20_{self.hand_type}_hand_state',10) if self.is_touch == True: if self.touch_type > 1: ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with matrix pressure sensing", color='green') - self.matrix_touch_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch', 10) - self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/cb_{self.hand_type}_hand_matrix_touch_pc', 10) - self.matrix_touch_mass_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch_mass', 10) + self.matrix_touch_pub = self.create_publisher(String, f'/l20_{self.hand_type}_hand_matrix_touch', 10) + self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/l20_{self.hand_type}_hand_matrix_touch_pc', 10) + self.matrix_touch_mass_pub = self.create_publisher(Float32MultiArray, f'/l20_{self.hand_type}_hand_matrix_touch_mass', 10) elif self.touch_type != -1: ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with pressure sensor", color="green") - self.touch_pub = self.create_publisher(Float32MultiArray, f'/cb_{self.hand_type}_hand_force', 10) + self.touch_pub = self.create_publisher(Float32MultiArray, f'/l20_{self.hand_type}_hand_force', 10) else: ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Not equipped with any pressure sensors", color="red") self.is_touch = False @@ -160,7 +160,7 @@ class LinkerHandAdvancedO6(Node): self.matrix_dic["little_matrix"] = self.api.get_little_matrix_touch(sleep_time=0.002).tolist() # 发布矩阵压感数据JSON格式 self.pub_matrix_dic() - # 发布矩阵压感和值JSON格式 + # 发布矩阵压感合值浮点数组 self.pub_matrix_mass(dic=self.matrix_dic) # 发布矩阵压感点云格式 self.pub_matrix_point_cloud() @@ -179,22 +179,15 @@ class LinkerHandAdvancedO6(Node): self.matrix_touch_pub.publish(msg) def pub_matrix_mass(self, dic): - """发布矩阵数据合值 单位g 克 JSON格式""" - msg = String() - # 获取当前的 ROS 时间 - current_time = self.stamp_clock.now() - # 提取 secs 和 nsecs - t_secs = current_time.to_msg().sec - t_nsecs = current_time.to_msg().nanosec - self.matrix_mass_dic["stamp"]["secs"] = t_secs - self.matrix_mass_dic["stamp"]["nsecs"] = t_nsecs - self.matrix_mass_dic["unit"] = "g" - self.matrix_mass_dic["thumb_mass"] = sum(sum(row) for row in dic["thumb_matrix"]) - self.matrix_mass_dic["index_mass"] = sum(sum(row) for row in dic["index_matrix"]) - self.matrix_mass_dic["middle_mass"] = sum(sum(row) for row in dic["middle_matrix"]) - self.matrix_mass_dic["ring_mass"] = sum(sum(row) for row in dic["ring_matrix"]) - self.matrix_mass_dic["little_mass"] = sum(sum(row) for row in dic["little_matrix"]) - msg.data = json.dumps(self.matrix_mass_dic) + """发布矩阵数据合值,顺序为 thumb, index, middle, ring, little,单位 g。""" + msg = Float32MultiArray() + msg.data = [ + float(sum(sum(row) for row in dic["thumb_matrix"])), + float(sum(sum(row) for row in dic["index_matrix"])), + float(sum(sum(row) for row in dic["middle_matrix"])), + float(sum(sum(row) for row in dic["ring_matrix"])), + float(sum(sum(row) for row in dic["little_matrix"])), + ] self.matrix_touch_mass_pub.publish(msg) def pub_matrix_point_cloud(self): @@ -226,9 +219,9 @@ class LinkerHandAdvancedO6(Node): def main(args=None): ''' 本节点用于收集手指状态和压感数据。 - '/cb_{self.hand_type}_hand_control_cmd' 话题类型为 sensor_msgs/msg/JointState 控制话题,限制 30Hz - /cb_{self.hand_type}_hand_state 话题类型为 sensor_msgs/msg/JointState 50Hz - '/cb_{self.hand_type}_hand_matrix_touch' 话题类型为 std_msgs/msg/String 50Hz + '/l20_{self.hand_type}_hand_control_cmd' 话题类型为 sensor_msgs/msg/JointState 控制话题,限制 30Hz + /l20_{self.hand_type}_hand_state 话题类型为 sensor_msgs/msg/JointState 50Hz + '/l20_{self.hand_type}_hand_matrix_touch' 话题类型为 std_msgs/msg/String 50Hz 启动命令: ros2 run linker_hand_ros2_sdk linker_hand_advanced_o6 --hand_type right --can can0 --is_touch true ''' diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_g20_palm_touch.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_g20_palm_touch.py index eda447c..8a19efc 100644 --- a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_g20_palm_touch.py +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_g20_palm_touch.py @@ -88,17 +88,17 @@ class LinkerHandAdvancedG20(Node): self.api = LinkerHandApi(hand_type=self.hand_type, hand_joint=self.hand_joint,modbus=self.modbus,can=self.can) time.sleep(0.1) self.touch_type = self.api.get_touch_type() - self.hand_cmd_sub = self.create_subscription(JointState, f'/cb_{self.hand_type}_hand_control_cmd', self.hand_control_cb,10) - self.hand_state_pub = self.create_publisher(JointState, f'/cb_{self.hand_type}_hand_state',10) + self.hand_cmd_sub = self.create_subscription(JointState, f'/l20_{self.hand_type}_hand_control_cmd', self.hand_control_cb,10) + self.hand_state_pub = self.create_publisher(JointState, f'/l20_{self.hand_type}_hand_state',10) if self.is_touch == True: if self.touch_type > 1: ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with matrix pressure sensing", color='green') - self.matrix_touch_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch', 10) - #self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/cb_{self.hand_type}_hand_matrix_touch_pc', 10) - self.matrix_touch_mass_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch_mass', 10) + self.matrix_touch_pub = self.create_publisher(String, f'/l20_{self.hand_type}_hand_matrix_touch', 10) + #self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/l20_{self.hand_type}_hand_matrix_touch_pc', 10) + self.matrix_touch_mass_pub = self.create_publisher(Float32MultiArray, f'/l20_{self.hand_type}_hand_matrix_touch_mass', 10) elif self.touch_type != -1: ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with pressure sensor", color="green") - self.touch_pub = self.create_publisher(Float32MultiArray, f'/cb_{self.hand_type}_hand_force', 10) + self.touch_pub = self.create_publisher(Float32MultiArray, f'/l20_{self.hand_type}_hand_force', 10) else: ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Not equipped with any pressure sensors", color="red") self.is_touch = False @@ -172,7 +172,7 @@ class LinkerHandAdvancedG20(Node): self.matrix_dic["palm_matrix"] = self.api.get_palm_matrix_touch(sleep_time=self.touch_sleep_time).tolist() # 发布矩阵压感数据JSON格式 self.pub_matrix_dic() - # 发布矩阵压感和值JSON格式 + # 发布矩阵压感合值浮点数组 self.pub_matrix_mass(dic=self.matrix_dic) # 发布矩阵压感点云格式 #self.pub_matrix_point_cloud() @@ -194,23 +194,15 @@ class LinkerHandAdvancedG20(Node): self.matrix_touch_pub.publish(msg) def pub_matrix_mass(self, dic): - """发布矩阵数据合值 单位g 克 JSON格式""" - msg = String() - # 获取当前的 ROS 时间 - current_time = self.stamp_clock.now() - # 提取 secs 和 nsecs - t_secs = current_time.to_msg().sec - t_nsecs = current_time.to_msg().nanosec - self.matrix_mass_dic["stamp"]["secs"] = t_secs - self.matrix_mass_dic["stamp"]["nsecs"] = t_nsecs - self.matrix_mass_dic["unit"] = "g" - self.matrix_mass_dic["thumb_mass"] = self.api.hand.thumb_matrix_palm_mass - self.matrix_mass_dic["index_mass"] = self.api.hand.index_matrix_palm_mass - self.matrix_mass_dic["middle_mass"] = self.api.hand.middle_matrix_palm_mass - self.matrix_mass_dic["ring_mass"] = self.api.hand.ring_matrix_palm_mass - self.matrix_mass_dic["little_mass"] = self.api.hand.little_matrix_palm_mass - self.matrix_mass_dic["palm_mass"] = self.api.hand.palm_matrix_palm_mass - msg.data = json.dumps(self.matrix_mass_dic) + """发布矩阵数据合值,顺序为 thumb, index, middle, ring, little,单位 g。""" + msg = Float32MultiArray() + msg.data = [ + float(sum(self.api.hand.thumb_matrix_palm_mass)), + float(sum(self.api.hand.index_matrix_palm_mass)), + float(sum(self.api.hand.middle_matrix_palm_mass)), + float(sum(self.api.hand.ring_matrix_palm_mass)), + float(sum(self.api.hand.little_matrix_palm_mass)), + ] self.matrix_touch_mass_pub.publish(msg) # def pub_matrix_point_cloud(self): @@ -242,9 +234,9 @@ class LinkerHandAdvancedG20(Node): def main(args=None): ''' 本节点用于收集手指状态和压感数据。 - '/cb_{self.hand_type}_hand_control_cmd' 话题类型为 sensor_msgs/msg/JointState 控制话题,限制 30Hz - /cb_{self.hand_type}_hand_state 话题类型为 sensor_msgs/msg/JointState 30Hz - '/cb_{self.hand_type}_hand_matrix_touch' 话题类型为 std_msgs/msg/String 30Hz + '/l20_{self.hand_type}_hand_control_cmd' 话题类型为 sensor_msgs/msg/JointState 控制话题,限制 30Hz + /l20_{self.hand_type}_hand_state 话题类型为 sensor_msgs/msg/JointState 30Hz + '/l20_{self.hand_type}_hand_matrix_touch' 话题类型为 std_msgs/msg/String 30Hz 启动命令: ros2 run linker_hand_ros2_sdk linker_hand_g20_palm_touch --hand_type left --can can0 --is_touch true ''' @@ -265,4 +257,4 @@ def main(args=None): node.close_can() # 关闭 CAN 或其他硬件资源 # node.destroy_node() # 销毁 ROS 节点 # rclpy.shutdown() # 关闭 ROS - print("程序已退出。") \ No newline at end of file + print("程序已退出。") diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/o6_joint_curve.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/o6_joint_curve.py index 2a06b5a..296bdc0 100644 --- a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/o6_joint_curve.py +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/o6_joint_curve.py @@ -145,7 +145,7 @@ class O6JointCurveRecorder(Node): self.saved_paths = None self.finished = False - topic = f"/cb_{hand_type}_hand_state" + topic = f"/l20_{hand_type}_hand_state" self.subscription = self.create_subscription( JointState, topic, self._state_callback, 100 )