diff --git a/src/openarm_joint_sensor/openarm_joint_sensor/__init__.py b/src/openarm_joint_sensor/openarm_joint_sensor/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/openarm_joint_sensor/openarm_joint_sensor/joint_control.py b/src/openarm_joint_sensor/openarm_joint_sensor/joint_control.py new file mode 100644 index 0000000..b71f94c --- /dev/null +++ b/src/openarm_joint_sensor/openarm_joint_sensor/joint_control.py @@ -0,0 +1,89 @@ +import rclpy +from rclpy.node import Node +# from control_msgs.msg import JointTrajectoryControllerState +from control_msgs.action import FollowJointTrajectory +from rclpy.action import ActionClient +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint +from sensor_msgs.msg import JointState + + +count = 0 +count1 = 0 +class ProgressActionClient(Node): + + + def __init__(self): + super().__init__('progress_action_client') + + self.subscriber_ = self.create_subscription( + JointState, + "/joint_sensor_states", + self.listener_callback, + 10 + ) + # 3-1.创建动作客户端； + self._action_client = ActionClient(self, FollowJointTrajectory, '/right_joint_trajectory_controller/follow_joint_trajectory') + + # 3. 创建订阅者 + # 参数1：消息类型（如String，与发布者一致） + # 参数2：话题名称（如"chatter"，与发布者一致） + # 参数3：回调函数（消息到达时触发，用于处理消息） + # 参数4：队列长度（10，缓存消息的最大数量） + + self.subscriber_ # 防止未使用变量警告 + + # 4. 消息处理回调函数（核心） + # 当订阅到消息时，自动调用此函数处理消息 + def listener_callback(self, msg): + + # self.send_goal([count, count1, 0.0, 0.0, 0.0, 0.0, 0.0]) + self.get_logger().info(f'Received joint states: {list(msg.name)} positions: {list(msg.position)}') + self.send_goal(msg.position) + + def send_goal(self, arm_pos): + arm_joint = ['openarm_right_joint1', 'openarm_right_joint2', 'openarm_right_joint3', 'openarm_right_joint4', 'openarm_right_joint5', 'openarm_right_joint6', 'openarm_right_joint7'] + arm_effort = [10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0] + arm_zero = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + # 3-2.发送请求； + goal_msg = FollowJointTrajectory.Goal() + goal_msg.trajectory.joint_names = arm_joint + goal_msg.trajectory.points.append(JointTrajectoryPoint()) + goal_msg.trajectory.points[0].positions = arm_pos + # goal_msg.trajectory.points[0].effort = arm_effort + # print(goal_msg) + + self._action_client.wait_for_server() + self._send_goal_future = self._action_client.send_goal_async(goal_msg, feedback_callback=self.feedback_callback) + self._send_goal_future.add_done_callback(self.goal_response_callback) + + def goal_response_callback(self, future): + # 3-3.处理目标发送后的反馈； + goal_handle = future.result() + print(goal_handle) + if not goal_handle.accepted: + self.get_logger().info('请求被拒绝') + return + + self.get_logger().info('请求被接收，开始执行任务！') + + self._get_result_future = goal_handle.get_result_async() + self._get_result_future.add_done_callback(self.get_result_callback) + + # 3-5.处理最终响应。 + def get_result_callback(self, future): + return + # result = future.result().result + # self.get_logger().info('最终计算结果：sum = %d' % result.sum) + # 5.释放资源。 + # rclpy.shutdown() + + # 3-4.处理连续反馈； + def feedback_callback(self, feedback_msg): + feedback = (int)(feedback_msg.feedback.progress * 100) + self.get_logger().info('当前进度: %d%%' % feedback) + +def main(): + rclpy.init() + node = ProgressActionClient() + rclpy.spin(node) + rclpy.shutdown() diff --git a/src/openarm_joint_sensor/openarm_joint_sensor/joint_moveit_control.py b/src/openarm_joint_sensor/openarm_joint_sensor/joint_moveit_control.py new file mode 100644 index 0000000..6afdead --- /dev/null +++ b/src/openarm_joint_sensor/openarm_joint_sensor/joint_moveit_control.py @@ -0,0 +1,52 @@ +import rclpy +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.node import Node + +from pymoveit2 import MoveIt2, MoveIt2State +from pymoveit2.robots import openarm as robot +from sensor_msgs.msg import JointState + +class MoveIt2ControlNode(Node): + def __init__(self): + super().__init__('moveit2_control_node') + self.get_logger().info('正在初始化MoveIt2控制节点...') + self.subscriber_ = self.create_subscription( + JointState, + "/joint_sensor_states", + self.listener_callback, + 10 + ) + # Create callback group that allows execution of callbacks in parallel without restrictions + callback_group = ReentrantCallbackGroup() + + # Create MoveIt 2 interface + self.moveit2 = MoveIt2( + node=self, + joint_names=robot.joint_names(), + base_link_name=robot.base_link_name(), + end_effector_name=robot.end_effector_name(), + group_name=robot.MOVE_GROUP_ARM, + callback_group=callback_group, + ) + self.moveit2.planner_id = ( + "RRTConnectkConfigDefault" + ) + # Scale down velocity and acceleration of joints (percentage of maximum) + self.moveit2.max_velocity = 0.5 + self.moveit2.max_acceleration = 0.5 + + def listener_callback(self, msg): + # self.send_goal([count, count1, 0.0, 0.0, 0.0, 0.0, 0.0]) + self.get_logger().info(f'Received joint states: positions: {list(msg.position)}') + self.moveit2.move_to_configuration(msg.position) + # self.get_logger().info("Waiting until execution is complete...") + self.moveit2.wait_until_executed() + +def main(): + rclpy.init() + node = MoveIt2ControlNode() + rclpy.spin(node) + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/openarm_joint_sensor/openarm_joint_sensor/joint_sensor_can.py b/src/openarm_joint_sensor/openarm_joint_sensor/joint_sensor_can.py new file mode 100644 index 0000000..fc5bfac --- /dev/null +++ b/src/openarm_joint_sensor/openarm_joint_sensor/joint_sensor_can.py @@ -0,0 +1,94 @@ +import rclpy +from rclpy.node import Node +import can +import math +from sensor_msgs.msg import JointState + +class CallbackListener(can.Listener): + def __init__(self, user_callback): + super().__init__() + self.user_callback = user_callback + + def on_message_received(self, msg: can.Message): + # 这是在 Notifier 的线程里调用的 -> 回调应该尽量快 + try: + self.user_callback(msg) + except Exception as e: + # 捕获回调内异常，避免中断 Notifier 分发 + print(f"Callback error: {e}") + + +class JointSensorNode(Node): + def __init__(self): + super().__init__('joint_sensor_node') + self.get_logger().info('正在初始化CAN接口...') + self.bus = can.interface.Bus(channel='can0', bustype='socketcan') + # ROS2 publisher: 发布 JointState 到话题 'joint_states' + self.publisher_ = self.create_publisher(JointState, 'joint_sensor_states', 10) + + # 可配置的 CAN ID -> joint name 列表映射 + # 默认示例：ID 0x123 包含两个关节角，每个角用 int16 存储（单位 centi-degrees，除以100得到度） + # 你可以根据真实硬件修改此映射或解析函数 + self.can_to_joints = { + 0x01: ['joint1'], + } + + self.listener = CallbackListener(self.listen_can_callback) + self.notifier = can.Notifier(self.bus, [self.listener]) + + def listen_can_callback(self, msg: can.Message): + # 用户自定义的回调函数 + try: + self.get_logger().debug(f"Received: id=0x{msg.arbitration_id:X} dlc={msg.dlc} data={msg.data.hex()} ts={msg.timestamp}") + + # 解析角度数据（默认按每 2 字节 int16，单位 centi-degrees） + angles = self.parse_angles_from_can(msg.data) + + # 根据 can id 找到 joint 名称列表 + names = self.can_to_joints.get(msg.arbitration_id, [f'joint_{i}' for i in range(len(angles))]) + + # 对长度不匹配做适配（截断或填充 0） + if len(names) != len(angles): + # 截断或填充名称或角度，保证二者等长 + if len(names) > len(angles): + names = names[:len(angles)] + else: + # 增加默认名称 + names = names + [f'joint_{i}' for i in range(len(names), len(angles))] + + # 发布 JointState（位置使用弧度） + js = JointState() + js.header.stamp = self.get_clock().now().to_msg() + js.name = names + js.position = angles + self.publisher_.publish(js) + + self.get_logger().info(f"Published JointState: ids=0x{msg.arbitration_id:X} names={names} positions={angles}") + except Exception as e: + self.get_logger().error(f"Error handling CAN msg: {e}") + + def shutdown(self): + self.notifier.stop() + self.bus.shutdown() + + def parse_angles_from_can(self, data: bytes): + angles = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # 默认两个关节角，单位弧度 + if data[0] == 0xaa: # 假设 0xaa 开头表示特殊格式，使用不同解析方式 + raw = (data[2] << 8) | data[3] + deg = (raw / 4096.0) * 360.0 # 例如特殊格式直接除以 4096 得到度 + rad = math.radians(deg) - 3.14 + self.get_logger().info(f"Special format detected: raw={raw} deg={deg} rad={rad}") + angles[0] = rad # 假设特殊格式只包含一个关节角，放在第一个位置 + return angles + + +def main(): + rclpy.init() + node = JointSensorNode() + rclpy.spin(node) + node.shutdown() + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/openarm_joint_sensor/package.xml b/src/openarm_joint_sensor/package.xml new file mode 100644 index 0000000..7f3500e --- /dev/null +++ b/src/openarm_joint_sensor/package.xml @@ -0,0 +1,18 @@ + + + + openarm_joint_sensor + 0.0.0 + TODO: Package description + shen + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/openarm_joint_sensor/resource/openarm_joint_sensor b/src/openarm_joint_sensor/resource/openarm_joint_sensor new file mode 100644 index 0000000..e69de29 diff --git a/src/openarm_joint_sensor/setup.cfg b/src/openarm_joint_sensor/setup.cfg new file mode 100644 index 0000000..02d4685 --- /dev/null +++ b/src/openarm_joint_sensor/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/openarm_joint_sensor +[install] +install_scripts=$base/lib/openarm_joint_sensor diff --git a/src/openarm_joint_sensor/setup.py b/src/openarm_joint_sensor/setup.py new file mode 100644 index 0000000..3309406 --- /dev/null +++ b/src/openarm_joint_sensor/setup.py @@ -0,0 +1,32 @@ +from setuptools import find_packages, setup + +package_name = 'openarm_joint_sensor' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='shen', + maintainer_email='664376944@qq.com', + description='TODO: Package description', + license='TODO: License declaration', + extras_require={ + 'test': [ + 'pytest', + ], + }, + entry_points={ + 'console_scripts': [ + 'joint_sensor_can = openarm_joint_sensor.joint_sensor_can:main', + 'joint_control = openarm_joint_sensor.joint_control:main', + 'joint_moveit_control = openarm_joint_sensor.joint_moveit_control:main', + ], + }, +) diff --git a/src/openarm_joint_sensor/test/test_copyright.py b/src/openarm_joint_sensor/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/src/openarm_joint_sensor/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/openarm_joint_sensor/test/test_flake8.py b/src/openarm_joint_sensor/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/src/openarm_joint_sensor/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/src/openarm_joint_sensor/test/test_pep257.py b/src/openarm_joint_sensor/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/src/openarm_joint_sensor/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings'