From d7af1d4dcca4eb62cae1c31f4242ad8a751b988e Mon Sep 17 00:00:00 2001
From: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com>
Date: Tue, 12 Mar 2024 17:11:06 +0100
Subject: [PATCH] Use ros2_control and add IMU to simulation (#77)
---
rae_description/urdf/rae.urdf.xacro | 41 +++++--
.../urdf/rae_gazebo_control.urdf.xacro | 20 ----
.../urdf/rae_ros2_control.urdf.xacro | 113 +++++++++++-------
rae_gazebo/CMakeLists.txt | 2 +-
rae_gazebo/config/gz_bridge.yaml | 39 ++++++
rae_gazebo/launch/gazebo.launch.py | 50 +++++---
rae_gazebo/package.xml | 3 +
7 files changed, 177 insertions(+), 91 deletions(-)
delete mode 100644 rae_description/urdf/rae_gazebo_control.urdf.xacro
create mode 100644 rae_gazebo/config/gz_bridge.yaml
diff --git a/rae_description/urdf/rae.urdf.xacro b/rae_description/urdf/rae.urdf.xacro
index f299473..a3e7b54 100644
--- a/rae_description/urdf/rae.urdf.xacro
+++ b/rae_description/urdf/rae.urdf.xacro
@@ -1,8 +1,9 @@
+
+
-
@@ -17,14 +18,23 @@
-
-
- 1
- 1
- true
- imu
-
-
+
+
+
+ 1
+ 10.0
+ true
+ rae/imu/data
+ rae_imu_frame
+ rae_imu_frame
+
+
+
+
+
+
+
@@ -150,6 +160,12 @@
+
+
+ 0.0001
+ 0.0001
+
+
@@ -184,6 +200,12 @@
+
+
+ 0.0001
+ 0.0001
+
+
@@ -331,5 +353,6 @@
+
diff --git a/rae_description/urdf/rae_gazebo_control.urdf.xacro b/rae_description/urdf/rae_gazebo_control.urdf.xacro
deleted file mode 100644
index b9fadf6..0000000
--- a/rae_description/urdf/rae_gazebo_control.urdf.xacro
+++ /dev/null
@@ -1,20 +0,0 @@
-
-
-
-
- odom
- base_link
- tf
- true
- true
- true
- odom
- left_wheel_joint
- right_wheel_joint
- 0.0854
- 0.0119
- 50
- cmd_vel
-
-
-
diff --git a/rae_description/urdf/rae_ros2_control.urdf.xacro b/rae_description/urdf/rae_ros2_control.urdf.xacro
index b6f3229..da0d399 100644
--- a/rae_description/urdf/rae_ros2_control.urdf.xacro
+++ b/rae_description/urdf/rae_ros2_control.urdf.xacro
@@ -1,51 +1,72 @@
-
-
- rae_hw/RaeHW
- left_wheel_joint
- right_wheel_joint
- /sys/class/pwm/pwmchip0
- 2
- 1
- 41
- 45
- 42
- 43
- 46
- 47
- 756
- 756
- 32
- 32
- 1
- 0.2
- 0.1
- 0.0005
- 1
- 0.2
- 0.1
- 0.0005
+
+
+
+
+ rae_hw/RaeHW
+ left_wheel_joint
+ right_wheel_joint
+ /sys/class/pwm/pwmchip0
+ 2
+ 1
+ 41
+ 45
+ 42
+ 43
+ 46
+ 47
+ 756
+ 756
+ 32
+ 32
+ 1
+ 0.2
+ 0.1
+ 0.0005
+ 1
+ 0.2
+ 0.1
+ 0.0005
- 30
- gpiochip0
-
-
-
- -1.68
- 1.68
-
-
-
-
-
-
- -1.68
- 1.68
-
-
-
-
-
+ 30
+ gpiochip0
+
+
+
+
+ ign_ros2_control/IgnitionSystem
+
+
+
+
+ -1.68
+ 1.68
+
+
+
+
+
+
+ -1.68
+ 1.68
+
+
+
+
+
+
+
+
+
+
+ $(find rae_hw)/config/controller.yaml
+
+ /diff_controller/cmd_vel_unstamped:=/cmd_vel
+
+
+
+
+
diff --git a/rae_gazebo/CMakeLists.txt b/rae_gazebo/CMakeLists.txt
index d956388..9db28b4 100644
--- a/rae_gazebo/CMakeLists.txt
+++ b/rae_gazebo/CMakeLists.txt
@@ -8,7 +8,7 @@ endif()
find_package(ament_cmake REQUIRED)
install(
- DIRECTORY launch worlds
+ DIRECTORY config launch worlds
DESTINATION share/${PROJECT_NAME}/
)
diff --git a/rae_gazebo/config/gz_bridge.yaml b/rae_gazebo/config/gz_bridge.yaml
new file mode 100644
index 0000000..b04f7a7
--- /dev/null
+++ b/rae_gazebo/config/gz_bridge.yaml
@@ -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
\ No newline at end of file
diff --git a/rae_gazebo/launch/gazebo.launch.py b/rae_gazebo/launch/gazebo.launch.py
index 9f2cdcd..9a5b31b 100644
--- a/rae_gazebo/launch/gazebo.launch.py
+++ b/rae_gazebo/launch/gazebo.launch.py
@@ -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/Image@ignition.msgs.Image',
- '/camera/depth_image@sensor_msgs/msg/Image@ignition.msgs.Image',
- '/camera/camera_info@sensor_msgs/msg/CameraInfo@ignition.msgs.CameraInfo',
-
- '/image_out@sensor_msgs/msg/Image@ignition.msgs.Image',
- '/image_tuning@sensor_msgs/msg/Image@ignition.msgs.Image',
-
- '/odom@nav_msgs/msg/Odometry@ignition.msgs.Odometry',
- '/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock',
- '/tf@tf2_msgs/msg/TFMessage[ignition.msgs.Pose_V',
- '/scan@sensor_msgs/msg/LaserScan@ignition.msgs.LaserScan',
- ],
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')],
)
]
diff --git a/rae_gazebo/package.xml b/rae_gazebo/package.xml
index 0322280..447576d 100644
--- a/rae_gazebo/package.xml
+++ b/rae_gazebo/package.xml
@@ -12,8 +12,11 @@
ament_lint_auto
ament_lint_common
+ rae_hw
ros_gz_bridge
ros_gz_sim
+ ign_ros2_control
+ robot_localization
ament_cmake