diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..a9b895f --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ + +/src/roarm_main/roarm_driver/roarm_driver/__pycache__/ diff --git a/README.md b/README.md index 6436e15..7f8e5d4 100644 --- a/README.md +++ b/README.md @@ -199,15 +199,19 @@ Check the serial devices again: You should now see a new device like `/dev/ttyUSB0` at the end of the list. If not, disconnect and reconnect the robotic arm. -### 3.2 Change the Serial Port Device -If the detected serial port device is `/dev/ttyUSB0`, you can skip this section and proceed to **3.3** Running the Robotic Arm Driver Node. +### 3.2 Update the Serial Port Device -If the serial port device is not `/dev/ttyUSB0`, you need to update the serial port device name in the Python script `~/roarm_ws_em0/src/roarm_main/roarm_driver/roarm_driver/roarm_driver.py` by changing line 15: +Grant read and write permissions to the serial device using the following command (replace `/dev/ttyUSB0` with your actual device path): + + sudo chmod 666 /dev/ttyUSB0 + +If the serial port device is not `/dev/ttyUSB0`, you can either provide the proper device name at launch and/or you can update the serial port device name in the launch file `~/roarm_ws_em0/src/roarm_main/roarm_driver/launch/roarm.launch.py` by changing line 10: - serial_port = "/dev/ttyUSB0" + default_value='/dev/ttyUSB0', to your actual serial port device name. -![image](images/roarm_driver.py.png) +editor + Then, recompile the ROS2 packages in the terminal: cd ~/roarm_ws_em0/ @@ -217,15 +221,13 @@ Then, recompile the ROS2 packages in the terminal: ### 3.3 Running the Robotic Arm Driver Node According to the ROS2 official documentation, it is not recommended to run ROS2 nodes in the same terminal where you compile the packages. Open a new terminal window using `Ctrl + Alt + T`. -Grant serial port permissions and run the ROS2 robotic arm driver node: - -Grant read and write permissions to the serial device using the following command (replace `/dev/ttyUSB0` with your actual device path): +Launch the driver node: - sudo chmod 666 /dev/ttyUSB0 + ros2 launch roarm_driver roarm.launch.py #if you have the default port or edited the launch file -Run the driver node: +Or, override the serial port name at launch with something like: - ros2 run roarm_driver roarm_driver + ros2 launch roarm_driver roarm.launch.py serial_port:=/dev/ttyUSB2 ### 3.4 Viewing the Model Joints Open a new terminal window with `Ctrl + Alt + T`. @@ -564,4 +566,50 @@ Call the service to make the robotic arm draw a circle at the specified position The x, y, and z parameters specify the center of the circle, and radius specifies the radius of the circle in meters. -By calling this service, you can control the robotic arm to draw a circle at the desired position. \ No newline at end of file +By calling this service, you can control the robotic arm to draw a circle at the desired position. + +## 9 Description Options ## + +The URDF description is embedded in xacro files to enable extended capabilities: +- Dynamically generate the latest description at launch-time +- Allow multiple copies of the arm to be loaded into a robot_description +- Allow the end-effector to be rotated +- Mount the arm on a robot + +### 9.1 Xacro Arguments ### +The arguments to the xacro files include: +- **prefix**. This parameter governs the prefix applied to the beginning of all links and joints. Because the TF framespace is global, multiple instances of the arm or robots can't have the same name. Defaults to nothing. +- **arm_base_link_name**. If you don't want to use a namespace to isolate the arm, this parameter allows you to rename the base link so it doesn't conflict with the robot's base_link. Defaults to base_link. +- **end_rot**. The final joint for the arm can be rotated by adjusting the clamp that holds the last servo. The factory configuration has the gripper opening to the right. To change the rotation, specifiy either 90, 180 or 270 degrees rotation. This is a CCW rotation when looking at the gripper. Defaults to 0. + +To run build the description, change to the roarm_description/urdf folder and run: + + xacro roarm_description.urdf.xacro + +This will compile the description and put it out to the terminal. It won't actually change anything. To experiment with the defaults run it like: + + xacro roarm_description.urdf.xacro prefix:=/left_arm/ end_rot:=180 + +This will cause all the link and joint names to start with /left_arm and flip the end effector to grip from the left. + +To save the compiled urdf, output it to roarm_generated.urdf: + + xacro roarm_description.urdf.xacro prefix:=/left_arm/ end_rot:=180 > roarm_generaated.urdf + +Remember to a fresh **colcon build** if not using the --symlink-install option. It's not necessary to save the changes if doing a live compile with a launch file: + +### 9.2 Launch Example of Two Mounted Arms ### + +To display two arms attached to a simplistic robot base, enabled with the same parameters, run: + + ros2 launch roarm_description mounted.display.launch.py end_rot:=90 + +Examine this launch file to understand how multiple arms could be added to a robot or how multiple robots with arms could exist in the same robot description. This command will launch the arms with the gripper/wrist rotated to open downward. + +This launch file defaults to an example namespace of /mybot1. All of the nodes it launches will share this namespace which can help create isolation needed to manage duplicates. + +In depth treatment of node isolation is beyond the scope of this example. To actually run two roarm_driver nodes you would need to isolate them with their own arm namespaces so they pick up their respective joints. The harder problem on a real robot might lie in reliabily assigning the correct serial port to each arm across reboots. + +This launch file compiles the xacro at launch time and loads the description as a parameter, pulling in the latest changes and arguments. If the robot base gets moved around in the environment the arms should move with it. + +Two Arms, One Bot \ No newline at end of file diff --git a/images/roarm.launch.py.png b/images/roarm.launch.py.png new file mode 100644 index 0000000..99f9c69 Binary files /dev/null and b/images/roarm.launch.py.png differ diff --git a/images/twoarms.PNG b/images/twoarms.PNG new file mode 100644 index 0000000..6bdfa25 Binary files /dev/null and b/images/twoarms.PNG differ diff --git a/src/roarm_main/roarm_description/config/mounted_roarm_description.rviz b/src/roarm_main/roarm_description/config/mounted_roarm_description.rviz new file mode 100644 index 0000000..76b3683 --- /dev/null +++ b/src/roarm_main/roarm_description/config/mounted_roarm_description.rviz @@ -0,0 +1,282 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /RobotModel1 + - /RobotModel1/Description Topic1 + - /TF1 + - /TF1/Frames1 + Splitter Ratio: 0.8294117450714111 + Tree Height: 439 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /mybot1/robot_description + Enabled: true + Links: + /mybot1/arm_left/base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + /mybot1/arm_left/gripper_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + /mybot1/arm_left/hand_tcp: + Alpha: 1 + Show Axes: false + Show Trail: false + /mybot1/arm_left/link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + /mybot1/arm_left/link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + /mybot1/arm_left/link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + /mybot1/arm_right/base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + /mybot1/arm_right/gripper_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + /mybot1/arm_right/hand_tcp: + Alpha: 1 + Show Axes: false + Show Trail: false + /mybot1/arm_right/link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + /mybot1/arm_right/link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + /mybot1/arm_right/link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + /mybot1/base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + mybot1/arm_left/base_link: + Value: true + mybot1/arm_left/gripper_link: + Value: true + mybot1/arm_left/hand_tcp: + Value: true + mybot1/arm_left/link1: + Value: true + mybot1/arm_left/link2: + Value: true + mybot1/arm_left/link3: + Value: true + mybot1/arm_right/base_link: + Value: true + mybot1/arm_right/gripper_link: + Value: true + mybot1/arm_right/hand_tcp: + Value: true + mybot1/arm_right/link1: + Value: true + mybot1/arm_right/link2: + Value: true + mybot1/arm_right/link3: + Value: true + mybot1/base_link: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: false + Show Axes: false + Show Names: false + Tree: + mybot1/base_link: + mybot1/arm_left/base_link: + mybot1/arm_left/link1: + mybot1/arm_left/link2: + mybot1/arm_left/link3: + mybot1/arm_left/gripper_link: + {} + mybot1/arm_left/hand_tcp: + {} + mybot1/arm_right/base_link: + mybot1/arm_right/link1: + mybot1/arm_right/link2: + mybot1/arm_right/link3: + mybot1/arm_right/gripper_link: + {} + mybot1/arm_right/hand_tcp: + {} + Update Interval: 0 + Value: true + Enabled: true + Global Options: + Background Color: 222; 221; 218 + Fixed Frame: mybot1/base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 1.4765546321868896 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.15409952402114868 + Y: 0.1464391052722931 + Z: 0.17184139788150787 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.28039857745170593 + Target Frame: + Value: Orbit (rviz) + Yaw: 5.69858980178833 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 736 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000015600000242fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000242000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000242fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000242000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004ba0000003efc0100000002fb0000000800540069006d00650100000000000004ba000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002490000024200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1210 + X: 702 + Y: 299 diff --git a/src/roarm_main/roarm_description/launch/display.launch.py b/src/roarm_main/roarm_description/launch/display.launch.py index 1bb3751..6817e63 100644 --- a/src/roarm_main/roarm_description/launch/display.launch.py +++ b/src/roarm_main/roarm_description/launch/display.launch.py @@ -7,7 +7,7 @@ def generate_launch_description(): package_name = 'roarm_description' - urdf_name = "roarm_description.urdf" + urdf_name = "roarm_generated.urdf" ld = LaunchDescription() pkg_share = FindPackageShare(package=package_name).find(package_name) urdf_model_path = os.path.join(pkg_share, f'urdf/{urdf_name}') diff --git a/src/roarm_main/roarm_description/launch/mounted.display.launch.py b/src/roarm_main/roarm_description/launch/mounted.display.launch.py new file mode 100644 index 0000000..5479bdb --- /dev/null +++ b/src/roarm_main/roarm_description/launch/mounted.display.launch.py @@ -0,0 +1,104 @@ +import os +from launch import LaunchDescription +from launch.substitutions import LaunchConfiguration, Command, PythonExpression +from launch.actions import DeclareLaunchArgument +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from launch_ros.parameter_descriptions import ParameterValue +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + + ld = LaunchDescription() + + # Declare prefix for namespace and tf tree elements + prefix_arg = DeclareLaunchArgument( + 'prefix', + default_value='/mybot1/', + description='Prefix for the arm links and joints' + ) + # When not namespacing the arm, changing the base link name gives a way to prepare the arm for merging onto a larger robot description + arm_base_link_name_arg = DeclareLaunchArgument( + 'arm_base_link_name', + default_value='base_link', + description="Name of the arm's base link" + ) + + # For arms where the end-effector has been rotated from the factory configuration + end_rot_arg = DeclareLaunchArgument( + 'end_rot', + default_value='0', + description="CCW rotation of the arm's end-effector (looking at it), can be 0, 90, 180, 270" + ) + + # Get the launch configuration variables + prefix = LaunchConfiguration('prefix') + base_link_name = LaunchConfiguration('arm_base_link_name') + end_rot = LaunchConfiguration('end_rot') + + # Define the path to the builder Xacro file + xacro_file = os.path.join( + get_package_share_directory('roarm_description'), + 'urdf', + #'roarm_urdf_builder.xacro' # use this if building your own cross-package robot description + 'mybot1.xacro' # use this for a minimal example of mounting the arm on a cylinder + ) + + # Process the Xacro file to generate the robot description parameter + robot_description_content = Command([ + 'xacro ', xacro_file, + ' prefix:=', prefix, + ' base_link_name:=', base_link_name, + ' end_rot:=', end_rot, + ]) + + # Wrap robot_description_content with ParameterValue + robot_description = {'robot_description': ParameterValue(robot_description_content, value_type=str)} + + # Construct the child_frame_id by concatenating prefix and base_link_name using PythonExpression + child_frame_id = PythonExpression(["'", prefix, "' + '", base_link_name, "'"]) + + + robot_state_publisher_node = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + namespace=[prefix], + output='screen', + parameters=[robot_description], + #arguments=['--ros-args', '--log-level', 'debug'], #if debugging needed + ) + + # some kind of joint state publisher is required for the arm joints or the + # tf tree won't be complete + # here using the gui tool + # otherwise joint states would be provided by the package used to control the arm + joint_state_publisher_node = Node( + package='joint_state_publisher_gui', + executable='joint_state_publisher_gui', + name='joint_state_publisher_gui', + namespace=[prefix], + parameters=[robot_description], + ) + + # RViz + rviz_config_file = ( + get_package_share_directory("roarm_description") + "/config/mounted_roarm_description.rviz" + ) + rviz2_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=["-d", rviz_config_file], + ) + + ld.add_action(prefix_arg) + ld.add_action(arm_base_link_name_arg) + ld.add_action(end_rot_arg) + ld.add_action(robot_state_publisher_node) + ld.add_action(joint_state_publisher_node) + ld.add_action(rviz2_node) + + return ld + diff --git a/src/roarm_main/roarm_description/meshes/link3_0.STL b/src/roarm_main/roarm_description/meshes/link3_0.STL new file mode 100644 index 0000000..52d3124 Binary files /dev/null and b/src/roarm_main/roarm_description/meshes/link3_0.STL differ diff --git a/src/roarm_main/roarm_description/meshes/link3_180.STL b/src/roarm_main/roarm_description/meshes/link3_180.STL new file mode 100644 index 0000000..bc016d0 Binary files /dev/null and b/src/roarm_main/roarm_description/meshes/link3_180.STL differ diff --git a/src/roarm_main/roarm_description/meshes/link3_270.STL b/src/roarm_main/roarm_description/meshes/link3_270.STL new file mode 100644 index 0000000..77fe981 Binary files /dev/null and b/src/roarm_main/roarm_description/meshes/link3_270.STL differ diff --git a/src/roarm_main/roarm_description/meshes/link3_90.STL b/src/roarm_main/roarm_description/meshes/link3_90.STL new file mode 100644 index 0000000..2b51218 Binary files /dev/null and b/src/roarm_main/roarm_description/meshes/link3_90.STL differ diff --git a/src/roarm_main/roarm_description/package.xml b/src/roarm_main/roarm_description/package.xml index bc7bf89..2bd3034 100644 --- a/src/roarm_main/roarm_description/package.xml +++ b/src/roarm_main/roarm_description/package.xml @@ -11,7 +11,7 @@ joint_state_publisher robot_state_publisher - rviz + rviz2 xacro rclcpp std_msgs diff --git a/src/roarm_main/roarm_description/urdf/mybot1.xacro b/src/roarm_main/roarm_description/urdf/mybot1.xacro new file mode 100644 index 0000000..f0fe0d1 --- /dev/null +++ b/src/roarm_main/roarm_description/urdf/mybot1.xacro @@ -0,0 +1,69 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/roarm_main/roarm_description/urdf/roarm_description.urdf.xacro b/src/roarm_main/roarm_description/urdf/roarm_description.urdf.xacro new file mode 100644 index 0000000..ff8843d --- /dev/null +++ b/src/roarm_main/roarm_description/urdf/roarm_description.urdf.xacro @@ -0,0 +1,299 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/roarm_main/roarm_description/urdf/roarm_generated.urdf b/src/roarm_main/roarm_description/urdf/roarm_generated.urdf new file mode 100644 index 0000000..bdc2312 --- /dev/null +++ b/src/roarm_main/roarm_description/urdf/roarm_generated.urdf @@ -0,0 +1,163 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/roarm_main/roarm_description/urdf/roarm_urdf_builder.xacro b/src/roarm_main/roarm_description/urdf/roarm_urdf_builder.xacro new file mode 100644 index 0000000..81240f2 --- /dev/null +++ b/src/roarm_main/roarm_description/urdf/roarm_urdf_builder.xacro @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/src/roarm_main/roarm_driver/launch/roarm.launch.py b/src/roarm_main/roarm_driver/launch/roarm.launch.py new file mode 100644 index 0000000..ceb2dbb --- /dev/null +++ b/src/roarm_main/roarm_driver/launch/roarm.launch.py @@ -0,0 +1,29 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration + +def generate_launch_description(): + # Declare the launch argument for serial port + serial_port_arg = DeclareLaunchArgument( + 'serial_port', + default_value='/dev/ttyUSB0', + description='Serial port to be used by the node' + ) + + # Declare the node with the serial port parameter + roarm_driver_node = Node( + package='roarm_driver', + executable='roarm_driver', + name='roarm_driver', + output='screen', + parameters=[{ + 'serial_port': LaunchConfiguration('serial_port'), + 'baud_rate': 115200 + }] + ) + + return LaunchDescription([ + serial_port_arg, + roarm_driver_node + ]) diff --git a/src/roarm_main/roarm_driver/roarm_driver/__pycache__/__init__.cpython-310.pyc b/src/roarm_main/roarm_driver/roarm_driver/__pycache__/__init__.cpython-310.pyc deleted file mode 100644 index 55e8d6b..0000000 Binary files a/src/roarm_main/roarm_driver/roarm_driver/__pycache__/__init__.cpython-310.pyc and /dev/null differ diff --git a/src/roarm_main/roarm_driver/roarm_driver/__pycache__/roarm_driver.cpython-310.pyc b/src/roarm_main/roarm_driver/roarm_driver/__pycache__/roarm_driver.cpython-310.pyc deleted file mode 100644 index 0c7848d..0000000 Binary files a/src/roarm_main/roarm_driver/roarm_driver/__pycache__/roarm_driver.cpython-310.pyc and /dev/null differ diff --git a/src/roarm_main/roarm_driver/roarm_driver/roarm_driver.py b/src/roarm_main/roarm_driver/roarm_driver/roarm_driver.py index d2a68ae..0e39580 100644 --- a/src/roarm_main/roarm_driver/roarm_driver/roarm_driver.py +++ b/src/roarm_main/roarm_driver/roarm_driver/roarm_driver.py @@ -12,8 +12,6 @@ import time import math -serial_port = "/dev/ttyUSB0" - class ReadLine: def __init__(self, s): self.buf = bytearray() @@ -84,11 +82,19 @@ class RoarmDriver(Node): def __init__(self): super().__init__('roarm_driver') - self.declare_parameter('serial_port', serial_port) + self.declare_parameter('serial_port', '/dev/ttyUSB0') self.declare_parameter('baud_rate', 115200) - serial_port_name = self.get_parameter('serial_port').get_parameter_value().string_value - baud_rate = self.get_parameter('baud_rate').get_parameter_value().integer_value + serial_port_name = self.get_parameter('serial_port').value + baud_rate = self.get_parameter('baud_rate').value + + # Initialize current positions for all joints (default to 0) + self.current_positions = { + 'base_link_to_link1': 0.0, + 'link1_to_link2': 0.0, + 'link2_to_link3': 0.0, + 'link3_to_gripper_link': 0.0 + } try: self.serial_port = serial.Serial(serial_port_name, baud_rate) @@ -99,7 +105,7 @@ def __init__(self): time.sleep(0.1) except SerialException as e: - self.get_logger().error(f"{serial_port_name}：{e}") + self.get_logger().error(f"{serial_port_name}: {e}") return self.joint_states_sub = self.create_subscription(JointState, 'joint_states', self.joint_states_callback,10) @@ -115,16 +121,33 @@ def joint_states_callback(self, msg): }, 'frame_id': msg.header.frame_id, } - + namespace = self.get_namespace() + # Normalize namespace by removing leading slash for comparison + namespace_normalized = namespace.lstrip('/') if namespace != "/" else "" + namespace_prefix = f"{namespace_normalized}/" if namespace_normalized else "" + name = msg.name position = msg.position velocity = msg.velocity effort = msg.effort - base = -position[name.index('base_link_to_link1')] - shoulder = -position[name.index('link1_to_link2')] - elbow = position[name.index('link2_to_link3')] - hand = 3.1415926 - position[name.index('link3_to_gripper_link')] + # Update only the joints that are present in this message + for i, joint_name in enumerate(name): + # Normalize joint name by removing leading slash before stripping namespace + joint_name_normalized = joint_name.lstrip('/') + # Remove namespace prefix to get base joint name + base_joint_name = joint_name_normalized.replace(namespace_prefix, "") + + # Update the position if this is one of our tracked joints + if base_joint_name in self.current_positions: + self.current_positions[base_joint_name] = position[i] + self.get_logger().debug(f"Updated {base_joint_name} to {position[i]:.3f}") + + # Use the current positions (which may be a mix of new and cached values) + base = -self.current_positions['base_link_to_link1'] + shoulder = -self.current_positions['link1_to_link2'] + elbow = self.current_positions['link2_to_link3'] + hand = 3.1415926 - self.current_positions['link3_to_gripper_link'] data = json.dumps({ 'T': 102, @@ -146,7 +169,7 @@ def pose_callback(self, msg): try: request_data = json.dumps({'T': 105}) + "\n" self.serial_port.write(request_data.encode()) - self.base_controller = BaseController(serial_port, 115200) + self.base_controller = BaseController(self.get_parameter('serial_port').value, 115200) time.sleep(0.1) self.base_controller.feedback_data() diff --git a/src/roarm_main/roarm_driver/setup.py b/src/roarm_main/roarm_driver/setup.py index 354660f..92b4278 100644 --- a/src/roarm_main/roarm_driver/setup.py +++ b/src/roarm_main/roarm_driver/setup.py @@ -1,4 +1,6 @@ from setuptools import find_packages, setup +import os +from glob import glob package_name = 'roarm_driver' @@ -7,9 +9,15 @@ version='0.0.0', packages=find_packages(exclude=['test']), data_files=[ + # This registers the package as a resource. ('share/ament_index/resource_index/packages', ['resource/' + package_name]), + + # Install the package.xml ('share/' + package_name, ['package.xml']), + + # Install all launch files into the share//launch directory + (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.*')), ], install_requires=['setuptools'], zip_safe=True, diff --git a/src/roarm_main/roarm_moveit_cmd/src/getposecmd_moveit2.cpp b/src/roarm_main/roarm_moveit_cmd/src/getposecmd_moveit2.cpp index 50ac5ca..b25a30f 100644 --- a/src/roarm_main/roarm_moveit_cmd/src/getposecmd_moveit2.cpp +++ b/src/roarm_main/roarm_moveit_cmd/src/getposecmd_moveit2.cpp @@ -2,8 +2,8 @@ #include #include #include -#include -#include +#include +#include #include #include #include diff --git a/src/roarm_main/roarm_moveit_cmd/src/getposecmd_tf2.cpp b/src/roarm_main/roarm_moveit_cmd/src/getposecmd_tf2.cpp index 54595a2..726cb69 100644 --- a/src/roarm_main/roarm_moveit_cmd/src/getposecmd_tf2.cpp +++ b/src/roarm_main/roarm_moveit_cmd/src/getposecmd_tf2.cpp @@ -2,9 +2,9 @@ #include #include #include -#include -#include -#include +#include +#include +#include #include "roarm_moveit/srv/get_pose_cmd.hpp" #include #include diff --git a/src/roarm_main/roarm_moveit_cmd/src/movecirclecmd.cpp b/src/roarm_main/roarm_moveit_cmd/src/movecirclecmd.cpp index bec88a9..4e8d07c 100644 --- a/src/roarm_main/roarm_moveit_cmd/src/movecirclecmd.cpp +++ b/src/roarm_main/roarm_moveit_cmd/src/movecirclecmd.cpp @@ -5,9 +5,9 @@ #include // 导入Eigen库，用于多项式拟合 #include "roarm_moveit_cmd/ik.h" #include -#include -#include -#include +#include +#include +#include #include "roarm_moveit/srv/move_circle_cmd.hpp" void generate_circle_trajectory(std::vector& waypoints, double center_x, double center_y, double radius, double resolution) { @@ -90,7 +90,7 @@ void handle_service(const std::shared_ptrsuccess = true; diff --git a/src/roarm_main/roarm_moveit_cmd/src/movepointcmd.cpp b/src/roarm_main/roarm_moveit_cmd/src/movepointcmd.cpp index 8826150..a77ddaf 100644 --- a/src/roarm_main/roarm_moveit_cmd/src/movepointcmd.cpp +++ b/src/roarm_main/roarm_moveit_cmd/src/movepointcmd.cpp @@ -1,9 +1,9 @@ #include #include #include -#include -#include -#include +#include +#include +#include #include "roarm_moveit/srv/move_point_cmd.hpp" #include "roarm_moveit_cmd/ik.h" void handle_service(const std::shared_ptr request, @@ -32,7 +32,7 @@ void handle_service(const std::shared_ptrsuccess = true; diff --git a/src/roarm_main/roarm_moveit_ikfast_plugins/package.xml b/src/roarm_main/roarm_moveit_ikfast_plugins/package.xml index e49959c..352c7be 100644 --- a/src/roarm_main/roarm_moveit_ikfast_plugins/package.xml +++ b/src/roarm_main/roarm_moveit_ikfast_plugins/package.xml @@ -1,23 +1,24 @@ - + roarm_moveit_ikfast_plugins 0.0.0 IKFast plugin for roarm_description BSD dudu + ament_cmake - - ament_cmake - - - moveit_core - pluginlib - rclcpp + + moveit_core + pluginlib + rclcpp + tf2_kdl tf2_eigen liblapack-dev - rclcpp - moveit_core - pluginlib liblapack-dev + + + ament_cmake + + diff --git a/src/roarm_main/roarm_moveit_ikfast_plugins/src/roarm_description_hand_ikfast_moveit_plugin.cpp b/src/roarm_main/roarm_moveit_ikfast_plugins/src/roarm_description_hand_ikfast_moveit_plugin.cpp index abb40f6..60d0a82 100644 --- a/src/roarm_main/roarm_moveit_ikfast_plugins/src/roarm_description_hand_ikfast_moveit_plugin.cpp +++ b/src/roarm_main/roarm_moveit_ikfast_plugins/src/roarm_description_hand_ikfast_moveit_plugin.cpp @@ -42,8 +42,8 @@ */ #include -#include -#include +#include +#include #include #include #include @@ -410,7 +410,9 @@ bool IKFastKinematicsPlugin::initialize(const rclcpp::Node::SharedPtr& node, } storeValues(robot_model, group_name, base_frame, tip_frames, search_discretization); - if (!lookupParam(node, "link_prefix", link_prefix_, std::string(""))) + node->declare_parameter("link_prefix", ""); + link_prefix_ = node->get_parameter("link_prefix").as_string(); + if (link_prefix_.empty()) { RCLCPP_INFO(LOGGER, "Using empty link_prefix."); }