From c67ee110051c649e7f1d83906c6194bb5b06af96 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 4 Feb 2022 21:31:28 +0100 Subject: [PATCH 1/5] Added Ignition Gazebo support Signed-off-by: ahcorde --- turtlebot3_ignition/CMakeLists.txt | 33 + .../config/turtlebot3_burger.yaml | 66 + .../config/turtlebot3_waffle.yaml | 66 + .../config/turtlebot3_waffle_pi.yaml | 66 + turtlebot3_ignition/gui/gui.config | 1264 +++++++++++++++++ turtlebot3_ignition/launch/control.launch.py | 67 + turtlebot3_ignition/launch/ignition.launch.py | 121 ++ .../launch/robot_state_publisher.launch.py | 53 + .../launch/ros_ign_bridge.launch.py | 68 + turtlebot3_ignition/launch/rviz.launch.py | 43 + turtlebot3_ignition/package.xml | 29 + turtlebot3_ignition/rviz/tb3_ignition.rviz | 244 ++++ turtlebot3_ignition/src/republisher_cmd.cpp | 49 + .../urdf/turtlebot3_burger.urdf | 9 + .../urdf/turtlebot3_ignition.urdf | 123 ++ .../urdf/turtlebot3_waffle.urdf | 9 + .../urdf/turtlebot3_waffle_pi.urdf | 9 + turtlebot3_ignition/worlds/empty.sdf | 71 + 18 files changed, 2390 insertions(+) create mode 100644 turtlebot3_ignition/CMakeLists.txt create mode 100644 turtlebot3_ignition/config/turtlebot3_burger.yaml create mode 100644 turtlebot3_ignition/config/turtlebot3_waffle.yaml create mode 100644 turtlebot3_ignition/config/turtlebot3_waffle_pi.yaml create mode 100644 turtlebot3_ignition/gui/gui.config create mode 100644 turtlebot3_ignition/launch/control.launch.py create mode 100644 turtlebot3_ignition/launch/ignition.launch.py create mode 100644 turtlebot3_ignition/launch/robot_state_publisher.launch.py create mode 100644 turtlebot3_ignition/launch/ros_ign_bridge.launch.py create mode 100644 turtlebot3_ignition/launch/rviz.launch.py create mode 100644 turtlebot3_ignition/package.xml create mode 100644 turtlebot3_ignition/rviz/tb3_ignition.rviz create mode 100644 turtlebot3_ignition/src/republisher_cmd.cpp create mode 100644 turtlebot3_ignition/urdf/turtlebot3_burger.urdf create mode 100644 turtlebot3_ignition/urdf/turtlebot3_ignition.urdf create mode 100644 turtlebot3_ignition/urdf/turtlebot3_waffle.urdf create mode 100644 turtlebot3_ignition/urdf/turtlebot3_waffle_pi.urdf create mode 100644 turtlebot3_ignition/worlds/empty.sdf diff --git a/turtlebot3_ignition/CMakeLists.txt b/turtlebot3_ignition/CMakeLists.txt new file mode 100644 index 00000000..c96670ff --- /dev/null +++ b/turtlebot3_ignition/CMakeLists.txt @@ -0,0 +1,33 @@ +################################################################################ +# Set minimum required version of cmake, project name and compile options +################################################################################ +cmake_minimum_required(VERSION 3.5) +project(turtlebot3_ignition) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +################################################################################ +# Find ament packages and libraries for ament and system dependencies +################################################################################ +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) + +install(DIRECTORY launch urdf config rviz gui worlds + DESTINATION share/${PROJECT_NAME} +) + +add_executable(cmd_republisher src/republisher_cmd.cpp) +ament_target_dependencies(cmd_republisher rclcpp geometry_msgs) + +install(TARGETS + cmd_republisher + DESTINATION lib/${PROJECT_NAME}) + +ament_package() diff --git a/turtlebot3_ignition/config/turtlebot3_burger.yaml b/turtlebot3_ignition/config/turtlebot3_burger.yaml new file mode 100644 index 00000000..94bd5db5 --- /dev/null +++ b/turtlebot3_ignition/config/turtlebot3_burger.yaml @@ -0,0 +1,66 @@ +controller_manager: + ros__parameters: + update_rate: 100 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + diff_drive_base_controller: + type: diff_drive_controller/DiffDriveController + + imu_sensor_broadcaster: + type: imu_sensor_broadcaster/IMUSensorBroadcaster + +imu_sensor_broadcaster: + ros__parameters: + sensor_name: imu_sensor + frame_id: imu + + +diff_drive_base_controller: + ros__parameters: + left_wheel_names: ["wheel_left_joint"] + right_wheel_names: ["wheel_right_joint"] + + wheel_separation: 0.160 + #wheels_per_side: 1 # actually 2, but both are controlled by 1 signal + wheel_radius: 0.033 + + wheel_separation_multiplier: 1.0 + left_wheel_radius_multiplier: 1.0 + right_wheel_radius_multiplier: 1.0 + + publish_rate: 50.0 + odom_frame_id: odom + base_frame_id: base_footprint + pose_covariance_diagonal : [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] + twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] + + open_loop: false + enable_odom_tf: true + + cmd_vel_timeout: 0.1 + #publish_limited_velocity: true + use_stamped_vel: false + #velocity_rolling_window_size: 10 + + # Velocity and acceleration limits + # Whenever a min_* is unspecified, default to -max_* + linear.x.has_velocity_limits: true + linear.x.has_acceleration_limits: true + linear.x.has_jerk_limits: false + linear.x.max_velocity: 1.0 + linear.x.min_velocity: -1.0 + linear.x.max_acceleration: 1.0 + linear.x.max_jerk: 0.0 + linear.x.min_jerk: 0.0 + + angular.z.has_velocity_limits: true + angular.z.has_acceleration_limits: true + angular.z.has_jerk_limits: false + angular.z.max_velocity: 1.0 + angular.z.min_velocity: -1.0 + angular.z.max_acceleration: 1.0 + angular.z.min_acceleration: -1.0 + angular.z.max_jerk: 0.0 + angular.z.min_jerk: 0.0 diff --git a/turtlebot3_ignition/config/turtlebot3_waffle.yaml b/turtlebot3_ignition/config/turtlebot3_waffle.yaml new file mode 100644 index 00000000..72e17f71 --- /dev/null +++ b/turtlebot3_ignition/config/turtlebot3_waffle.yaml @@ -0,0 +1,66 @@ +controller_manager: + ros__parameters: + update_rate: 100 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + diff_drive_base_controller: + type: diff_drive_controller/DiffDriveController + + imu_sensor_broadcaster: + type: imu_sensor_broadcaster/IMUSensorBroadcaster + +imu_sensor_broadcaster: + ros__parameters: + sensor_name: imu_sensor + frame_id: imu + + +diff_drive_base_controller: + ros__parameters: + left_wheel_names: ["wheel_left_joint"] + right_wheel_names: ["wheel_right_joint"] + + wheel_separation: 0.287 + #wheels_per_side: 1 # actually 2, but both are controlled by 1 signal + wheel_radius: 0.033 + + wheel_separation_multiplier: 1.0 + left_wheel_radius_multiplier: 1.0 + right_wheel_radius_multiplier: 1.0 + + publish_rate: 50.0 + odom_frame_id: odom + base_frame_id: base_footprint + pose_covariance_diagonal : [0.001, 0.001, 0.0, 0.0, 0.0, 0.01] + twist_covariance_diagonal: [0.001, 0.0, 0.0, 0.0, 0.0, 0.01] + + open_loop: false + enable_odom_tf: true + + cmd_vel_timeout: 0.1 + #publish_limited_velocity: true + use_stamped_vel: false + #velocity_rolling_window_size: 10 + + # Velocity and acceleration limits + # Whenever a min_* is unspecified, default to -max_* + linear.x.has_velocity_limits: true + linear.x.has_acceleration_limits: true + linear.x.has_jerk_limits: false + linear.x.max_velocity: 1.0 + linear.x.min_velocity: -1.0 + linear.x.max_acceleration: 1.0 + linear.x.max_jerk: 0.0 + linear.x.min_jerk: 0.0 + + angular.z.has_velocity_limits: true + angular.z.has_acceleration_limits: true + angular.z.has_jerk_limits: false + angular.z.max_velocity: 1.0 + angular.z.min_velocity: -1.0 + angular.z.max_acceleration: 1.0 + angular.z.min_acceleration: -1.0 + angular.z.max_jerk: 0.0 + angular.z.min_jerk: 0.0 diff --git a/turtlebot3_ignition/config/turtlebot3_waffle_pi.yaml b/turtlebot3_ignition/config/turtlebot3_waffle_pi.yaml new file mode 100644 index 00000000..72e17f71 --- /dev/null +++ b/turtlebot3_ignition/config/turtlebot3_waffle_pi.yaml @@ -0,0 +1,66 @@ +controller_manager: + ros__parameters: + update_rate: 100 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + diff_drive_base_controller: + type: diff_drive_controller/DiffDriveController + + imu_sensor_broadcaster: + type: imu_sensor_broadcaster/IMUSensorBroadcaster + +imu_sensor_broadcaster: + ros__parameters: + sensor_name: imu_sensor + frame_id: imu + + +diff_drive_base_controller: + ros__parameters: + left_wheel_names: ["wheel_left_joint"] + right_wheel_names: ["wheel_right_joint"] + + wheel_separation: 0.287 + #wheels_per_side: 1 # actually 2, but both are controlled by 1 signal + wheel_radius: 0.033 + + wheel_separation_multiplier: 1.0 + left_wheel_radius_multiplier: 1.0 + right_wheel_radius_multiplier: 1.0 + + publish_rate: 50.0 + odom_frame_id: odom + base_frame_id: base_footprint + pose_covariance_diagonal : [0.001, 0.001, 0.0, 0.0, 0.0, 0.01] + twist_covariance_diagonal: [0.001, 0.0, 0.0, 0.0, 0.0, 0.01] + + open_loop: false + enable_odom_tf: true + + cmd_vel_timeout: 0.1 + #publish_limited_velocity: true + use_stamped_vel: false + #velocity_rolling_window_size: 10 + + # Velocity and acceleration limits + # Whenever a min_* is unspecified, default to -max_* + linear.x.has_velocity_limits: true + linear.x.has_acceleration_limits: true + linear.x.has_jerk_limits: false + linear.x.max_velocity: 1.0 + linear.x.min_velocity: -1.0 + linear.x.max_acceleration: 1.0 + linear.x.max_jerk: 0.0 + linear.x.min_jerk: 0.0 + + angular.z.has_velocity_limits: true + angular.z.has_acceleration_limits: true + angular.z.has_jerk_limits: false + angular.z.max_velocity: 1.0 + angular.z.min_velocity: -1.0 + angular.z.max_acceleration: 1.0 + angular.z.min_acceleration: -1.0 + angular.z.max_jerk: 0.0 + angular.z.min_jerk: 0.0 diff --git a/turtlebot3_ignition/gui/gui.config b/turtlebot3_ignition/gui/gui.config new file mode 100644 index 00000000..5cb6b84a --- /dev/null +++ b/turtlebot3_ignition/gui/gui.config @@ -0,0 +1,1264 @@ + + + + -1 + -1 + + 944 + 845 +