From 4cec9044652c49d0277850b29fac9222db2d5599 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Fri, 22 Nov 2024 12:46:35 +0100 Subject: [PATCH] Fix multiple robot spawn --- docker/.env | 2 +- docker/Dockerfile | 7 +- docker/compose.yaml | 2 +- docker/justfile | 2 +- rosbot/rosbot_hardware.repos | 4 + rosbot_controller/launch/controller.launch.py | 1 + rosbot_controller/test/test_xacro.py | 3 +- rosbot_gazebo/config/gz_remappings.yaml | 20 ++- rosbot_gazebo/launch/simulation.launch.py | 169 +++++++----------- rosbot_gazebo/launch/spawn.launch.py | 96 +++++++--- rosbot_utils/rosbot_utils/flash-firmware.py | 4 +- rosbot_utils/rosbot_utils/flash_firmware.py | 2 +- 12 files changed, 169 insertions(+), 143 deletions(-) diff --git a/docker/.env b/docker/.env index 7725261e..9a9566ef 100644 --- a/docker/.env +++ b/docker/.env @@ -1,2 +1,2 @@ SERIAL_PORT=/dev/ttyUSB0 -ROS_NAMESPACE=robot1 \ No newline at end of file +ROS_NAMESPACE=robot1 diff --git a/docker/Dockerfile b/docker/Dockerfile index d89cffbe..858e67ff 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -14,9 +14,8 @@ RUN apt-get update && apt-get install -y \ ros-${ROS_DISTRO}-teleop-twist-keyboard RUN vcs import src < src/rosbot/rosbot_hardware.repos && \ - # cp -r src/ros2_controllers/diff_drive_controller src/ && \ - # cp -r src/ros2_controllers/imu_sensor_broadcaster src/ && \ - # rm -rf src/ros2_controllers && \ + cp -r src/ros2_controllers/imu_sensor_broadcaster src/ && \ + rm -rf src/ros2_controllers && \ rm -r src/rosbot_gazebo # Create a script to install runtime dependencies for final image @@ -29,4 +28,4 @@ RUN apt update && \ source /opt/ros/$ROS_DISTRO/setup.bash && \ colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release && \ echo $(cat /ros2_ws/src/rosbot/package.xml | grep '' | sed -r 's/.*([0-9]+.[0-9]+.[0-9]+)<\/version>/\1/g') >> /version.txt && \ - rm -rf build log src \ No newline at end of file + rm -rf build log src diff --git a/docker/compose.yaml b/docker/compose.yaml index a4fff438..fb9aaf96 100644 --- a/docker/compose.yaml +++ b/docker/compose.yaml @@ -16,4 +16,4 @@ services: mecanum:=${MECANUM:-False} serial_port:=$SERIAL_PORT serial_baudrate:=576000 - # namespace:=${ROS_NAMESPACE:-rosbot} \ No newline at end of file + # namespace:=${ROS_NAMESPACE:-rosbot} diff --git a/docker/justfile b/docker/justfile index 9003edaa..50fbe34d 100644 --- a/docker/justfile +++ b/docker/justfile @@ -86,4 +86,4 @@ _install-yq: curl -L https://github.com/mikefarah/yq/releases/download/${YQ_VERSION}/yq_linux_${YQ_ARCH} -o /usr/bin/yq chmod +x /usr/bin/yq echo "yq installed successfully!" - fi \ No newline at end of file + fi diff --git a/rosbot/rosbot_hardware.repos b/rosbot/rosbot_hardware.repos index 1a054c48..b50ee636 100644 --- a/rosbot/rosbot_hardware.repos +++ b/rosbot/rosbot_hardware.repos @@ -19,3 +19,7 @@ repositories: type: git url: https://github.com/micro-ROS/micro-ROS-Agent.git version: humble + ros2_controllers: # Bug: There is no nice way to change `sensor_name` imu_bradcaster param when spawning multiple robots -> ros2_control refer only to single imu entity + type: git + url: https://github.com/husarion/ros2_controllers/ + version: 9da42a07a83bbf3faf5cad11793fff22f25068af diff --git a/rosbot_controller/launch/controller.launch.py b/rosbot_controller/launch/controller.launch.py index 9ae2e69e..e5edf532 100644 --- a/rosbot_controller/launch/controller.launch.py +++ b/rosbot_controller/launch/controller.launch.py @@ -27,6 +27,7 @@ from launch_ros.actions import Node, SetParameter from launch_ros.substitutions import FindPackageShare + def generate_launch_description(): namespace = LaunchConfiguration("namespace") declare_namespace_arg = DeclareLaunchArgument( diff --git a/rosbot_controller/test/test_xacro.py b/rosbot_controller/test/test_xacro.py index 61e12688..7e06d41e 100644 --- a/rosbot_controller/test/test_xacro.py +++ b/rosbot_controller/test/test_xacro.py @@ -45,6 +45,5 @@ def test_rosbot_description_parsing(): xacro.process_file(xacro_path, mappings=mappings) except xacro.XacroException as e: assert False, ( - f"xacro parsing failed: {str(e)} for mecanum: {mecanum}, use_sim:" - f" {use_sim}" + f"xacro parsing failed: {str(e)} for mecanum: {mecanum}, use_sim:" f" {use_sim}" ) diff --git a/rosbot_gazebo/config/gz_remappings.yaml b/rosbot_gazebo/config/gz_remappings.yaml index 550cc139..25bb71a7 100644 --- a/rosbot_gazebo/config/gz_remappings.yaml +++ b/rosbot_gazebo/config/gz_remappings.yaml @@ -4,48 +4,58 @@ gz_type_name: ignition.msgs.Clock direction: GZ_TO_ROS + - topic_name: /cmd_vel + ros_type_name: geometry_msgs/msg/Twist + gz_type_name: ignition.msgs.Twist + 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: /camera/color/camera_info ros_type_name: sensor_msgs/msg/CameraInfo gz_type_name: ignition.msgs.CameraInfo - lazy: true + direction: GZ_TO_ROS - topic_name: /camera/color/image_raw ros_type_name: sensor_msgs/msg/Image gz_type_name: ignition.msgs.Image - lazy: true + direction: GZ_TO_ROS - topic_name: /camera/depth/camera_info ros_type_name: sensor_msgs/msg/CameraInfo gz_type_name: ignition.msgs.CameraInfo - lazy: true + direction: GZ_TO_ROS - topic_name: /camera/depth/image_raw ros_type_name: sensor_msgs/msg/Image gz_type_name: ignition.msgs.Image - lazy: true + direction: GZ_TO_ROS - ros_topic_name: /camera/depth/points gz_topic_name: /camera/depth/image_raw/points ros_type_name: sensor_msgs/msg/PointCloud2 gz_type_name: ignition.msgs.PointCloudPacked - lazy: true + direction: GZ_TO_ROS - topic_name: /range/fl ros_type_name: sensor_msgs/msg/LaserScan gz_type_name: ignition.msgs.LaserScan + direction: GZ_TO_ROS - topic_name: /range/fr ros_type_name: sensor_msgs/msg/LaserScan gz_type_name: ignition.msgs.LaserScan + direction: GZ_TO_ROS - topic_name: /range/rl ros_type_name: sensor_msgs/msg/LaserScan gz_type_name: ignition.msgs.LaserScan + direction: GZ_TO_ROS - topic_name: /range/rr ros_type_name: sensor_msgs/msg/LaserScan gz_type_name: ignition.msgs.LaserScan + direction: GZ_TO_ROS diff --git a/rosbot_gazebo/launch/simulation.launch.py b/rosbot_gazebo/launch/simulation.launch.py index 3123e94e..44c92a71 100644 --- a/rosbot_gazebo/launch/simulation.launch.py +++ b/rosbot_gazebo/launch/simulation.launch.py @@ -13,13 +13,7 @@ # limitations under the License. from launch import LaunchDescription -from launch.actions import ( - DeclareLaunchArgument, - GroupAction, - IncludeLaunchDescription, - LogInfo, - OpaqueFunction, -) +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, TimerAction from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import ( EnvironmentVariable, @@ -32,34 +26,46 @@ from nav2_common.launch import ParseMultiRobotPose -def launch_setup(context, *args, **kwargs): - namespace = LaunchConfiguration("namespace").perform(context) - mecanum = LaunchConfiguration("mecanum").perform(context) - world = LaunchConfiguration("world").perform(context) - headless = LaunchConfiguration("headless").perform(context) - x = LaunchConfiguration("x", default="0.0").perform(context) - y = LaunchConfiguration("y", default="2.0").perform(context) - z = LaunchConfiguration("z", default="0.0").perform(context) - roll = LaunchConfiguration("roll", default="0.0").perform(context) - pitch = LaunchConfiguration("pitch", default="0.0").perform(context) - yaw = LaunchConfiguration("yaw", default="0.0").perform(context) +def generate_launch_description(): + namespace = LaunchConfiguration("namespace") + mecanum = LaunchConfiguration("mecanum") + x = LaunchConfiguration("x", default="0.0") + y = LaunchConfiguration("y", default="2.0") + z = LaunchConfiguration("z", default="0.0") + roll = LaunchConfiguration("roll", default="0.0") + pitch = LaunchConfiguration("pitch", default="0.0") + yaw = LaunchConfiguration("yaw", default="0.0") + + declare_namespace_arg = DeclareLaunchArgument( + "namespace", + default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""), + description="Namespace for all topics and tfs", + ) + + declare_mecanum_arg = DeclareLaunchArgument( + "mecanum", + default_value="False", + description=( + "Whether to use mecanum drive controller (otherwise diff drive controller is used)" + ), + ) - gz_args = f"--headless-rendering -s -v 4 -r {world}" if eval(headless) else f"-r {world}" + declare_robots_arg = DeclareLaunchArgument( + "robots", + default_value="", + description=( + "Spawning multiple robots at positions with yaw orientations e.g." + "robots:='robot1={x: 0.0, y: -1.0}; robot2={x: 1.0, y: -1.0}; robot3={x: 2.0, y: -1.0};'" + ), + ) gz_sim = IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution( - [ - FindPackageShare("ros_gz_sim"), - "launch", - "gz_sim.launch.py", - ] + [FindPackageShare("husarion_gz_worlds"), "launch", "gz_sim.launch.py"] ) ), - launch_arguments={ - "gz_args": gz_args, - "on_exit_shutdown": "True", - }.items(), + launch_arguments={"gz_log_level": "1"}.items(), ) robots_list = ParseMultiRobotPose("robots").value() @@ -74,94 +80,47 @@ def launch_setup(context, *args, **kwargs): "yaw": yaw, } } + else: + for robot_name in robots_list: + init_pose = robots_list[robot_name] + for key, value in init_pose.items(): + init_pose[key] = TextSubstitution(text=str(value)) spawn_group = [] - for robot_name in robots_list: + for idx, robot_name in enumerate(robots_list): init_pose = robots_list[robot_name] - group = GroupAction( - [ - LogInfo( - msg=[ - "Launching namespace=", - robot_name, - " init_pose=", - str(init_pose), + spawn_robot = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare("rosbot_gazebo"), + "launch", + "spawn.launch.py", ] - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - FindPackageShare("rosbot_gazebo"), - "launch", - "spawn.launch.py", - ] - ) - ), - launch_arguments={ - "mecanum": mecanum, - "use_sim": "True", - "namespace": TextSubstitution(text=robot_name), - "x": TextSubstitution(text=str(init_pose["x"])), - "y": TextSubstitution(text=str(init_pose["y"])), - "z": TextSubstitution(text=str(init_pose["z"])), - "roll": TextSubstitution(text=str(init_pose["roll"])), - "pitch": TextSubstitution(text=str(init_pose["pitch"])), - "yaw": TextSubstitution(text=str(init_pose["yaw"])), - }.items(), - ), - ] + ) + ), + launch_arguments={ + "mecanum": mecanum, + "use_sim": "True", + "namespace": robot_name, + "x": init_pose["x"], + "y": init_pose["y"], + "z": init_pose["z"], + "roll": init_pose["roll"], + "pitch": init_pose["pitch"], + "yaw": init_pose["yaw"], + }.items(), ) - spawn_group.append(group) - - return [gz_sim, *spawn_group] - - -def generate_launch_description(): - declare_namespace_arg = DeclareLaunchArgument( - "namespace", - default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""), - description="Namespace for all topics and tfs", - ) - - declare_mecanum_arg = DeclareLaunchArgument( - "mecanum", - default_value="False", - description=( - "Whether to use mecanum drive controller (otherwise diff drive controller is used)" - ), - ) - - world_package = FindPackageShare("husarion_gz_worlds") - world_file = PathJoinSubstitution([world_package, "worlds", "husarion_world.sdf"]) - declare_world_arg = DeclareLaunchArgument( - "world", default_value=world_file, description="SDF world file" - ) - - declare_headless_arg = DeclareLaunchArgument( - "headless", - default_value="False", - description="Run Gazebo Ignition in the headless mode", - choices=["True", "False"], - ) - - declare_robots_arg = DeclareLaunchArgument( - "robots", - default_value="", - description=( - "Spawning multiple robots at positions with yaw orientations e. g. robots:='robot1={x:" - " 0.0, y: -1.0}; robot2={x: 1.0, y: -1.0}; robot3={x: 2.0, y: -1.0};'" - ), - ) + spawn_robot_delay = TimerAction(period=5.0 * idx, actions=[spawn_robot]) + spawn_group.append(spawn_robot_delay) return LaunchDescription( [ declare_namespace_arg, declare_mecanum_arg, - declare_world_arg, - declare_headless_arg, declare_robots_arg, SetParameter(name="use_sim_time", value=True), - OpaqueFunction(function=launch_setup), + gz_sim, + *spawn_group, ] ) diff --git a/rosbot_gazebo/launch/spawn.launch.py b/rosbot_gazebo/launch/spawn.launch.py index 0fa7123a..64134d39 100644 --- a/rosbot_gazebo/launch/spawn.launch.py +++ b/rosbot_gazebo/launch/spawn.launch.py @@ -13,7 +13,7 @@ # limitations under the License. from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, LogInfo from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import ( LaunchConfiguration, @@ -26,28 +26,55 @@ def generate_launch_description(): + mecanum = LaunchConfiguration("mecanum") namespace = LaunchConfiguration("namespace") + x = LaunchConfiguration("x") + y = LaunchConfiguration("y") + z = LaunchConfiguration("z") + roll = LaunchConfiguration("roll") + pitch = LaunchConfiguration("pitch") + yaw = LaunchConfiguration("yaw") + + declare_mecanum_arg = DeclareLaunchArgument( + "mecanum", + default_value="False", + description=( + "Whether to use mecanum drive controller (otherwise diff drive controller is used)" + ), + ) + declare_namespace_arg = DeclareLaunchArgument( "namespace", default_value="", description="Namespace for all topics and tfs", ) - namespace_ext = PythonExpression( - ["''", " if '", namespace, "' == '' ", "else ", "'/", namespace, "'"] + declare_x_arg = DeclareLaunchArgument( + "x", default_value="0.0", description="Initial robot position in the global 'x' axis." ) - mecanum = LaunchConfiguration("mecanum") - declare_mecanum_arg = DeclareLaunchArgument( - "mecanum", - default_value="False", - description=( - "Whether to use mecanum drive controller (otherwise diff drive controller is used)" - ), + declare_y_arg = DeclareLaunchArgument( + "y", default_value="-2.0", description="Initial robot position in the global 'y' axis." ) - robot_name = PythonExpression( - ["'rosbot'", " if '", namespace, "' == '' ", "else ", "'", namespace, "'"] + declare_z_arg = DeclareLaunchArgument( + "z", default_value="0.0", description="Initial robot position in the global 'z' axis." + ) + + declare_roll_arg = DeclareLaunchArgument( + "roll", default_value="0.0", description="Initial robot 'roll' orientation." + ) + + declare_pitch_arg = DeclareLaunchArgument( + "pitch", default_value="0.0", description="Initial robot 'pitch' orientation." + ) + + declare_yaw_arg = DeclareLaunchArgument( + "yaw", default_value="0.0", description="Initial robot 'yaw' orientation." + ) + + namespace_ext = PythonExpression( + ["''", " if '", namespace, "' == '' ", "else ", "'/", namespace, "'"] ) gz_remappings_file = PathJoinSubstitution( @@ -64,28 +91,47 @@ def generate_launch_description(): executable="create", arguments=[ "-name", - robot_name, + namespace, "-allow_renaming", "true", "-topic", "robot_description", "-x", - LaunchConfiguration("x", default="0.00"), + x, "-y", - LaunchConfiguration("y", default="0.00"), + y, "-z", - LaunchConfiguration("z", default="0.00"), + z, "-R", - LaunchConfiguration("roll", default="0.00"), + roll, "-P", - LaunchConfiguration("pitch", default="0.00"), + pitch, "-Y", - LaunchConfiguration("yaw", default="0.00"), + yaw, ], - output="screen", namespace=namespace, ) + welcome_msg = LogInfo( + msg=[ + "Spawning ROSbot\n\tNamespace: '", + namespace, + "'\n\tInitial pose: (", + x, + ", ", + y, + ", ", + z, + ", ", + roll, + ", ", + pitch, + ", ", + yaw, + ")", + ] + ) + ign_bridge = Node( package="ros_gz_bridge", executable="parameter_bridge", @@ -95,7 +141,6 @@ def generate_launch_description(): ("/tf", "tf"), ("/tf_static", "tf_static"), ], - output="screen", namespace=namespace, ) @@ -118,9 +163,16 @@ def generate_launch_description(): return LaunchDescription( [ - declare_namespace_arg, declare_mecanum_arg, + declare_namespace_arg, + declare_x_arg, + declare_y_arg, + declare_z_arg, + declare_roll_arg, + declare_pitch_arg, + declare_yaw_arg, SetParameter(name="use_sim_time", value=True), + welcome_msg, ign_bridge, gz_spawn_entity, bringup_launch, diff --git a/rosbot_utils/rosbot_utils/flash-firmware.py b/rosbot_utils/rosbot_utils/flash-firmware.py index 10f0901f..dd16d2b8 100755 --- a/rosbot_utils/rosbot_utils/flash-firmware.py +++ b/rosbot_utils/rosbot_utils/flash-firmware.py @@ -25,6 +25,7 @@ import gpiod import sh + def get_raspberry_pi_model(): try: with open("/proc/cpuinfo", "r") as f: @@ -40,6 +41,7 @@ def get_raspberry_pi_model(): except FileNotFoundError: return "Not a Raspberry Pi" + class FirmwareFlasher: def __init__(self, sys_arch, binary_file): self.binary_file = binary_file @@ -77,7 +79,7 @@ def __init__(self, sys_arch, binary_file): gpio_chip = "/dev/gpiochip4" else: gpio_chip = "/dev/gpiochip0" # Default or error handling - + boot0_pin_no = 17 reset_pin_no = 18 else: diff --git a/rosbot_utils/rosbot_utils/flash_firmware.py b/rosbot_utils/rosbot_utils/flash_firmware.py index e9338b5e..cbaeaa77 100755 --- a/rosbot_utils/rosbot_utils/flash_firmware.py +++ b/rosbot_utils/rosbot_utils/flash_firmware.py @@ -116,4 +116,4 @@ def main(args=None): if __name__ == "__main__": - main() \ No newline at end of file + main()