-
Notifications
You must be signed in to change notification settings - Fork 13
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Use ros2_control and add IMU to simulation (#77)
- Loading branch information
Showing
7 changed files
with
177 additions
and
91 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file was deleted.
Oops, something went wrong.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,51 +1,72 @@ | ||
| <?xml version="1.0"?> | ||
| <robot name="rae" | ||
| xmlns:xacro="http://ros.org/wiki/xacro"> | ||
| <ros2_control name="RAE" type="system"> | ||
| <hardware> | ||
| <plugin>rae_hw/RaeHW</plugin> | ||
| <param name="left_wheel_name">left_wheel_joint</param> | ||
| <param name="right_wheel_name">right_wheel_joint</param> | ||
| <param name="pwmName">/sys/class/pwm/pwmchip0</param> | ||
| <param name="pwmL">2</param> | ||
| <param name="pwmR">1</param> | ||
| <param name="phL">41</param> | ||
| <param name="phR">45</param> | ||
| <param name="enLA">42</param> | ||
| <param name="enLB">43</param> | ||
| <param name="enRA">46</param> | ||
| <param name="enRB">47</param> | ||
| <param name="encTicsPerRevL">756</param> | ||
| <param name="encTicsPerRevR">756</param> | ||
| <param name="maxVelL">32</param> | ||
| <param name="maxVelR">32</param> | ||
| <param name="closed_loopL">1</param> | ||
| <param name="PID_P_L">0.2</param> | ||
| <param name="PID_I_L">0.1</param> | ||
| <param name="PID_D_L">0.0005</param> | ||
| <param name="closed_loopR">1</param> | ||
| <param name="PID_P_R">0.2</param> | ||
| <param name="PID_I_R">0.1</param> | ||
| <param name="PID_D_R">0.0005</param> | ||
| <xacro:macro name="rae_ros2_control" params="sim_mode"> | ||
| <ros2_control name="RAE" type="system"> | ||
| <xacro:unless value="${sim_mode}"> | ||
| <hardware> | ||
| <plugin>rae_hw/RaeHW</plugin> | ||
| <param name="left_wheel_name">left_wheel_joint</param> | ||
| <param name="right_wheel_name">right_wheel_joint</param> | ||
| <param name="pwmName">/sys/class/pwm/pwmchip0</param> | ||
| <param name="pwmL">2</param> | ||
| <param name="pwmR">1</param> | ||
| <param name="phL">41</param> | ||
| <param name="phR">45</param> | ||
| <param name="enLA">42</param> | ||
| <param name="enLB">43</param> | ||
| <param name="enRA">46</param> | ||
| <param name="enRB">47</param> | ||
| <param name="encTicsPerRevL">756</param> | ||
| <param name="encTicsPerRevR">756</param> | ||
| <param name="maxVelL">32</param> | ||
| <param name="maxVelR">32</param> | ||
| <param name="closed_loopL">1</param> | ||
| <param name="PID_P_L">0.2</param> | ||
| <param name="PID_I_L">0.1</param> | ||
| <param name="PID_D_L">0.0005</param> | ||
| <param name="closed_loopR">1</param> | ||
| <param name="PID_P_R">0.2</param> | ||
| <param name="PID_I_R">0.1</param> | ||
| <param name="PID_D_R">0.0005</param> | ||
|
|
||
| <param name="loop_rate">30</param> | ||
| <param name="chip_name">gpiochip0</param> | ||
| </hardware> | ||
| <joint name="left_wheel_joint"> | ||
| <command_interface name="velocity"> | ||
| <param name="min">-1.68</param> | ||
| <param name="max">1.68</param> | ||
| </command_interface> | ||
| <state_interface name="velocity"/> | ||
| <state_interface name="position"/> | ||
| </joint> | ||
| <joint name="right_wheel_joint"> | ||
| <command_interface name="velocity"> | ||
| <param name="min">-1.68</param> | ||
| <param name="max">1.68</param> | ||
| </command_interface> | ||
| <state_interface name="velocity"/> | ||
| <state_interface name="position"/> | ||
| </joint> | ||
| </ros2_control> | ||
| <param name="loop_rate">30</param> | ||
| <param name="chip_name">gpiochip0</param> | ||
| </hardware> | ||
| </xacro:unless> | ||
| <xacro:if value="${sim_mode}"> | ||
| <hardware> | ||
| <plugin>ign_ros2_control/IgnitionSystem</plugin> | ||
| </hardware> | ||
| </xacro:if> | ||
| <joint name="left_wheel_joint"> | ||
| <command_interface name="velocity"> | ||
| <param name="min">-1.68</param> | ||
| <param name="max">1.68</param> | ||
| </command_interface> | ||
| <state_interface name="velocity"/> | ||
| <state_interface name="position"/> | ||
| </joint> | ||
| <joint name="right_wheel_joint"> | ||
| <command_interface name="velocity"> | ||
| <param name="min">-1.68</param> | ||
| <param name="max">1.68</param> | ||
| </command_interface> | ||
| <state_interface name="velocity"/> | ||
| <state_interface name="position"/> | ||
| </joint> | ||
| </ros2_control> | ||
|
|
||
|
|
||
| <xacro:if value="${sim_mode}"> | ||
| <gazebo> | ||
| <plugin filename="ign_ros2_control-system" name="ign_ros2_control::IgnitionROS2ControlPlugin"> | ||
| <parameters>$(find rae_hw)/config/controller.yaml</parameters> | ||
| <ros> | ||
| <remapping>/diff_controller/cmd_vel_unstamped:=/cmd_vel</remapping> | ||
| </ros> | ||
| </plugin> | ||
| </gazebo> | ||
| </xacro:if> | ||
| </xacro:macro> | ||
| </robot> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,39 @@ | ||
| - topic_name: "/clock" | ||
| ros_type_name: "rosgraph_msgs/msg/Clock" | ||
| gz_type_name: "ignition.msgs.Clock" | ||
| direction: GZ_TO_ROS | ||
|
|
||
| - topic_name: "/camera/image" | ||
| ros_type_name: "sensor_msgs/msg/Image" | ||
| gz_type_name: "ignition.msgs.Image" | ||
| direction: GZ_TO_ROS | ||
|
|
||
| - topic_name: "/camera/depth_image" | ||
| ros_type_name: "sensor_msgs/msg/Image" | ||
| gz_type_name: "ignition.msgs.Image" | ||
| direction: GZ_TO_ROS | ||
|
|
||
| - topic_name: "/camera/camera_info" | ||
| ros_type_name: "sensor_msgs/msg/CameraInfo" | ||
| gz_type_name: "ignition.msgs.CameraInfo" | ||
| direction: GZ_TO_ROS | ||
|
|
||
| - topic_name: "/image_out" | ||
| ros_type_name: "sensor_msgs/msg/Image" | ||
| gz_type_name: "ignition.msgs.Image" | ||
| direction: GZ_TO_ROS | ||
|
|
||
| - topic_name: "/image_tuning" | ||
| ros_type_name: "sensor_msgs/msg/Image" | ||
| gz_type_name: "ignition.msgs.Image" | ||
| direction: GZ_TO_ROS | ||
|
|
||
| - topic_name: "/scan" | ||
| ros_type_name: "sensor_msgs/msg/LaserScan" | ||
| gz_type_name: "ignition.msgs.LaserScan" | ||
| direction: GZ_TO_ROS | ||
|
|
||
| - topic_name: "/rae/imu/data" | ||
| ros_type_name: "sensor_msgs/msg/Imu" | ||
| gz_type_name: "ignition.msgs.IMU" | ||
| direction: GZ_TO_ROS |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -7,11 +7,14 @@ | |
| from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction, SetEnvironmentVariable | ||
| from launch.launch_description_sources import PythonLaunchDescriptionSource | ||
| from launch.substitutions import LaunchConfiguration | ||
| from launch.conditions import IfCondition | ||
| from launch_ros.actions import Node | ||
|
|
||
| def launch_setup(context, *args, **kwargs): | ||
| rae_description_pkg = get_package_share_directory('rae_description') | ||
| ros_gz_sim_pkg = get_package_share_directory('ros_gz_sim') | ||
| enable_localization = LaunchConfiguration( | ||
| 'enable_localization', default='true') | ||
| world_file = LaunchConfiguration('sdf_file').perform(context) | ||
| print(f'world_file: {world_file}') | ||
| return [ | ||
|
|
@@ -47,22 +50,10 @@ def launch_setup(context, *args, **kwargs): | |
| Node( | ||
| package='ros_gz_bridge', | ||
| executable='parameter_bridge', | ||
| arguments=[ | ||
| '/cmd_vel@geometry_msgs/msg/Twist]ignition.msgs.Twist', | ||
| '/camera/image@sensor_msgs/msg/[email protected]', | ||
| '/camera/depth_image@sensor_msgs/msg/[email protected]', | ||
| '/camera/camera_info@sensor_msgs/msg/[email protected]', | ||
|
|
||
| '/image_out@sensor_msgs/msg/[email protected]', | ||
| '/image_tuning@sensor_msgs/msg/[email protected]', | ||
|
|
||
| '/odom@nav_msgs/msg/[email protected]', | ||
| '/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock', | ||
| '/tf@tf2_msgs/msg/TFMessage[ignition.msgs.Pose_V', | ||
| '/scan@sensor_msgs/msg/[email protected]', | ||
| ], | ||
| parameters=[{ | ||
| 'use_sim_time': True | ||
| 'use_sim_time': True, | ||
| 'config_file': os.path.join(get_package_share_directory( | ||
| 'rae_gazebo'),'config', 'gz_bridge.yaml') | ||
| }], | ||
| output='screen' | ||
| ), | ||
|
|
@@ -75,6 +66,35 @@ def launch_setup(context, *args, **kwargs): | |
| '-topic', 'robot_description', | ||
| ], | ||
| output='screen' | ||
| ), | ||
|
|
||
| # Activate diff controller | ||
| Node( | ||
| package='controller_manager', | ||
| namespace=LaunchConfiguration('namespace'), | ||
| executable='spawner', | ||
| arguments=['diff_controller', '-c', '/controller_manager'], | ||
| ), | ||
|
|
||
| # Activate joint state broadcaster | ||
| Node( | ||
| package='controller_manager', | ||
| executable='spawner', | ||
| namespace=LaunchConfiguration('namespace'), | ||
| arguments=['joint_state_broadcaster', | ||
| '--controller-manager', '/controller_manager'], | ||
| ), | ||
|
|
||
| # Activate EKF | ||
| Node( | ||
| condition=IfCondition(enable_localization), | ||
| package='robot_localization', | ||
| executable='ekf_node', | ||
| name='ekf_filter_node', | ||
| namespace=LaunchConfiguration('namespace'), | ||
| output='screen', | ||
| parameters=[os.path.join(get_package_share_directory( | ||
| 'rae_hw'), 'config', 'ekf.yaml')], | ||
| ) | ||
| ] | ||
|
|
||
|
|
||
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters