From: Walter Fetter Lages Date: Mon, 2 Dec 2024 02:59:35 +0000 (-0300) Subject: Port to Jazzy ros2_control. X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=37f3bf480552762753192563f26e9217a759c148;p=q2d.git Port to Jazzy ros2_control. --- diff --git a/q2d_bringup/CMakeLists.txt b/q2d_bringup/CMakeLists.txt index 3b6c960..c2aed6d 100644 --- a/q2d_bringup/CMakeLists.txt +++ b/q2d_bringup/CMakeLists.txt @@ -11,7 +11,7 @@ find_package(ament_cmake REQUIRED) install(PROGRAMS scripts/ijc_step.sh scripts/ijc_square.py - scripts/group_torque_step.sh + scripts/torque_step.sh scripts/joint_trajectory_step.sh DESTINATION lib/${PROJECT_NAME} ) diff --git a/q2d_bringup/config/bypass.yaml b/q2d_bringup/config/bypass.yaml index 29d9a17..52309c1 100644 --- a/q2d_bringup/config/bypass.yaml +++ b/q2d_bringup/config/bypass.yaml @@ -1,11 +1,6 @@ -shoulder_controller: +bypass_controller: ros__parameters: - joints: + type: effort_controllers/JointGroupEffortController + joints: - shoulder_active_joint - interface_name: effort - -elbow_controller: - ros__parameters: - joints: - elbow_active_joint - interface_name: effort diff --git a/q2d_bringup/config/computed_torque.yaml b/q2d_bringup/config/computed_torque.yaml index c16f2f0..6168684 100644 --- a/q2d_bringup/config/computed_torque.yaml +++ b/q2d_bringup/config/computed_torque.yaml @@ -1,5 +1,6 @@ computed_torque_controller: ros__parameters: + type: computed_torque_controller/ComputedTorqueController joints: - shoulder_active_joint - elbow_active_joint diff --git a/q2d_bringup/config/group_bypass.yaml b/q2d_bringup/config/group_bypass.yaml deleted file mode 100644 index 0413e10..0000000 --- a/q2d_bringup/config/group_bypass.yaml +++ /dev/null @@ -1,5 +0,0 @@ -group_bypass: - ros__parameters: - joints: - - shoulder_active_joint - - elbow_active_joint diff --git a/q2d_bringup/config/group_pid.yaml b/q2d_bringup/config/group_pid.yaml deleted file mode 100644 index 1bcbd0b..0000000 --- a/q2d_bringup/config/group_pid.yaml +++ /dev/null @@ -1,7 +0,0 @@ -group_controller: - ros__parameters: - joints: - - shoulder_active_joint - - elbow_active_joint - shoulder_active_joint/pid: {p: 2310, i: 4640, d: 0.299} - elbow_active_joint/pid: {p: 339, i: 851, d: 0.351} diff --git a/q2d_bringup/config/joint_state_broadcaster.yaml b/q2d_bringup/config/joint_state_broadcaster.yaml new file mode 100644 index 0000000..29015ca --- /dev/null +++ b/q2d_bringup/config/joint_state_broadcaster.yaml @@ -0,0 +1,3 @@ +joint_state_broadcaster: + ros__parameters: + type: joint_state_broadcaster/JointStateBroadcaster diff --git a/q2d_bringup/config/pid.yaml b/q2d_bringup/config/pid.yaml index 962b7f6..61ee10a 100644 --- a/q2d_bringup/config/pid.yaml +++ b/q2d_bringup/config/pid.yaml @@ -1,9 +1,11 @@ -shoulder_controller: +pid_controller: ros__parameters: - joint: shoulder_active_joint - pid: {p: 2310, i: 4640, d: 0.299} - -elbow_controller: - ros__parameters: - joint: elbow_active_joint - pid: {p: 339, i: 851, d: 0.351} + type: pid_controller/PidController + dof_names: + - shoulder_active_joint + - elbow_active_joint + command_interface: effort + reference_and_state_interfaces: ["position"] + gains: + shoulder_active_joint: {p: 2310.0, i: 4640.0, d: 0.299, i_clamp_max: 100.0, i_clamp_min: -100.0, antiwindup: false} + elbow_active_joint: {p: 339.0, i: 841.0, d: 0.351, i_clamp_max: 100.0, i_clamp_min: -100.0, antiwindup: false} diff --git a/q2d_bringup/config/pid_plus_gravity.yaml b/q2d_bringup/config/pid_plus_gravity.yaml deleted file mode 100644 index a0ad17a..0000000 --- a/q2d_bringup/config/pid_plus_gravity.yaml +++ /dev/null @@ -1,10 +0,0 @@ -pid_plus_gravity_controller: - ros__parameters: - joints: - - shoulder_active_joint - - elbow_active_joint - shoulder_active_joint: {p: 2310.0, i: 4640.0, d: 0.299, i_clamp_max: 27.94, i_clamp_min: -27.94} - elbow_active_joint: {p: 339.0, i: 851.0, d: 0.351, i_clamp_max: 13.62, i_clamp_min: -13.62} - gravity: {x: 0.0, y: 0.0, z: -9.8} - chain: {root: "origin_link", tip: "tool_link"} - priority: 99 diff --git a/q2d_bringup/launch/bypass.launch.xml b/q2d_bringup/launch/bypass.launch.xml index 046bfbe..9a8977b 100644 --- a/q2d_bringup/launch/bypass.launch.xml +++ b/q2d_bringup/launch/bypass.launch.xml @@ -1,7 +1,7 @@ - + - - + - - - - - - diff --git a/q2d_bringup/launch/group_bypass.launch.xml b/q2d_bringup/launch/group_bypass.launch.xml deleted file mode 100644 index 6683992..0000000 --- a/q2d_bringup/launch/group_bypass.launch.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - - - - - - - diff --git a/q2d_bringup/launch/group_pid.launch.xml b/q2d_bringup/launch/group_pid.launch.xml deleted file mode 100644 index 74bfa6a..0000000 --- a/q2d_bringup/launch/group_pid.launch.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - - - - - - - diff --git a/q2d_bringup/launch/hardware.launch.xml b/q2d_bringup/launch/hardware.launch.xml index 9eccbd7..26d4dd8 100644 --- a/q2d_bringup/launch/hardware.launch.xml +++ b/q2d_bringup/launch/hardware.launch.xml @@ -22,7 +22,7 @@ - + @@ -32,11 +32,11 @@ - + + args="-p $(find-pkg-share q2d_bringup)/config/joint_state_broadcaster.yaml joint_state_broadcaster"/> diff --git a/q2d_bringup/launch/pid.launch.xml b/q2d_bringup/launch/pid.launch.xml index 98e279f..ba31b9d 100644 --- a/q2d_bringup/launch/pid.launch.xml +++ b/q2d_bringup/launch/pid.launch.xml @@ -1,7 +1,7 @@ - - - - - - - - diff --git a/q2d_bringup/scripts/ijc_square.py b/q2d_bringup/scripts/ijc_square.py index 38c2178..d1db84c 100755 --- a/q2d_bringup/scripts/ijc_square.py +++ b/q2d_bringup/scripts/ijc_square.py @@ -1,29 +1,31 @@ #!/usr/bin/python3 import sys +import array import rclpy from rclpy.node import Node -from std_msgs.msg import Float64 +from control_msgs.msg import MultiDOFCommand class SquarePublisher(Node): def __init__(self): super().__init__('square_publisher') - self.publisher_=self.create_publisher(Float64,sys.argv[1],1) - timer_period=float(sys.argv[4]) + self.publisher_=self.create_publisher(MultiDOFCommand,"/pid_controller/reference",1) + timer_period=float(sys.argv[5]) self.timer=self.create_timer(timer_period,self.timer_callback) self.turn=True; def timer_callback(self): - ref=Float64() - ref.data=float(sys.argv[2] if self.turn else sys.argv[3]) + ref=MultiDOFCommand() + ref.dof_names=["shoulder_active_joint", "elbow_active_joint"] + ref.values=[float(sys.argv[1] if self.turn else sys.argv[2]), float(sys.argv[3] if self.turn else sys.argv[4])] self.turn=not self.turn self.publisher_.publish(ref) def main(args=None): if len(sys.argv) < 5: - print('square.py topic min max period') + print('ijc_square.py shoulder_min shoulder_max elbow_min elbow_max period') exit() rclpy.init(args=args) diff --git a/q2d_bringup/scripts/ijc_step.sh b/q2d_bringup/scripts/ijc_step.sh index 26f69df..c6f573b 100755 --- a/q2d_bringup/scripts/ijc_step.sh +++ b/q2d_bringup/scripts/ijc_step.sh @@ -1,4 +1,8 @@ #!/bin/bash -ros2 topic pub /shoulder_controller/command std_msgs/msg/Float64 "{data: $1}" -1 & -ros2 topic pub /elbow_controller/command std_msgs/msg/Float64 "{data: $2}" -1 +if [ "$#" -ne 2 ]; then + echo "Usage: $0 >" + exit -1; +fi; + +ros2 topic pub /pid_controller/reference control_msgs/msg/MultiDOFCommand "{dof_names: ["shoulder_active_joint", "elbow_active_joint"], values: [$1, $2]}" -1 diff --git a/q2d_bringup/scripts/group_torque_step.sh b/q2d_bringup/scripts/torque_step.sh similarity index 54% rename from q2d_bringup/scripts/group_torque_step.sh rename to q2d_bringup/scripts/torque_step.sh index 5b61a7e..b822ff0 100755 --- a/q2d_bringup/scripts/group_torque_step.sh +++ b/q2d_bringup/scripts/torque_step.sh @@ -5,4 +5,4 @@ if [ "$#" -ne 2 ]; then exit -1; fi; -ros2 topic pub /group_controller/commands std_msgs/msg/Float64MultiArray "{data: [$1, $2] }" -1 +ros2 topic pub /bypass_controller/commands std_msgs/msg/Float64MultiArray "{data: [$1, $2] }" -1 diff --git a/q2d_description/CMakeLists.txt b/q2d_description/CMakeLists.txt index 8bc8c3c..cd6ed07 100644 --- a/q2d_description/CMakeLists.txt +++ b/q2d_description/CMakeLists.txt @@ -26,6 +26,4 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() -ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/env-hooks/${PROJECT_NAME}.dsv.in") - ament_package() diff --git a/q2d_description/config/gz_bridge.yaml b/q2d_description/config/gz_bridge.yaml new file mode 100644 index 0000000..2154629 --- /dev/null +++ b/q2d_description/config/gz_bridge.yaml @@ -0,0 +1,10 @@ +- ros_topic_name: "/clock" + gz_topic_name: "/clock" + ros_type_name: "rosgraph_msgs/msg/Clock" + gz_type_name: "ignition.msgs.Clock" +# subscriber_queue: 5 # Default 10 +# publisher_queue: 6 # Default 10 +# lazy: true # Default "false" + direction: GZ_TO_ROS # Default "BIDIRECTIONAL" - Bridge both directions + # "GZ_TO_ROS" - Bridge Ignition topic to ROS + # "ROS_TO_GZ" - Bridge ROS topic to Ignition diff --git a/q2d_description/env-hooks/q2d_description.dsv.in b/q2d_description/env-hooks/q2d_description.dsv.in deleted file mode 100644 index 30a6001..0000000 --- a/q2d_description/env-hooks/q2d_description.dsv.in +++ /dev/null @@ -1,3 +0,0 @@ -prepend-non-duplicate;IGN_GAZEBO_RESOURCE_PATH;share - -set-if-unset;MESA_GL_VERSION_OVERRIDE;3.3 diff --git a/q2d_description/launch/display.launch.py b/q2d_description/launch/display.launch.py index d3745f6..b5ac5a0 100644 --- a/q2d_description/launch/display.launch.py +++ b/q2d_description/launch/display.launch.py @@ -48,7 +48,7 @@ def generate_launch_description(): package='tf2_ros', executable='static_transform_publisher', name='q2d_origin_publisher', - arguments=["--frame-id", "map","--child-frame-id","origin_link"]), + arguments=["--frame-id", "map","--child-frame-id","World"]), Node( package='joint_state_publisher_gui', executable='joint_state_publisher_gui', diff --git a/q2d_description/launch/display.launch.xml b/q2d_description/launch/display.launch.xml index 807304d..5848fc0 100644 --- a/q2d_description/launch/display.launch.xml +++ b/q2d_description/launch/display.launch.xml @@ -21,7 +21,7 @@ - + diff --git a/q2d_description/launch/gazebo.launch.xml b/q2d_description/launch/gazebo.launch.xml index 356ab65..0260838 100644 --- a/q2d_description/launch/gazebo.launch.xml +++ b/q2d_description/launch/gazebo.launch.xml @@ -20,7 +20,7 @@ *******************************************************************************--> - + @@ -41,7 +41,8 @@ - + + diff --git a/q2d_description/urdf/q2d.urdf b/q2d_description/urdf/q2d.urdf index b55f348..abe7be3 100644 --- a/q2d_description/urdf/q2d.urdf +++ b/q2d_description/urdf/q2d.urdf @@ -4,8 +4,15 @@ + + + + + + + @@ -174,26 +181,6 @@ - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - 1 - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - 1 - - - @@ -251,13 +238,11 @@ - - ign_ros2_control/IgnitionSystem + gz_ros2_control/GazeboSimSystem - q2d_hardware/Q2dSystemHardware @@ -285,14 +270,9 @@ - - - - robot_description - robot_state_publisher - $(find q2d_description)/config/controller_manager.yaml - - + + $(find q2d_description)/config/controller_manager.yaml + diff --git a/q2d_teleop/launch/gazebo.launch.xml b/q2d_teleop/launch/gazebo.launch.xml index cc3cdfa..3857a75 100644 --- a/q2d_teleop/launch/gazebo.launch.xml +++ b/q2d_teleop/launch/gazebo.launch.xml @@ -1,7 +1,7 @@ - - -