html5-img
1 / 47

Multi-Robot Systems with ROS Lesson 8

Multi-Robot Systems with ROS Lesson 8. Teaching Assistant: Roi Yehoshua roiyeho@gmail.com. Agenda. Running navigation stack in Gazebo with multiple robots Sending goals to robots in Gazebo Using amcl Multi robot collision avoidance in navigation stack. Meet Lizi.

lilith
Download Presentation

Multi-Robot Systems with ROS Lesson 8

An Image/Link below is provided (as is) to download presentation Download Policy: Content on the Website is provided to you AS IS for your information and personal use and may not be sold / licensed / shared on other websites without getting consent from its author. Content is provided to you AS IS for your information and personal use only. Download presentation by click this link. While downloading, if for some reason you are not able to download a presentation, the publisher may have deleted the file from their server. During download, if you can't get a presentation, the file might be deleted by the publisher.

E N D

Presentation Transcript

  1. Multi-Robot Systems with ROS Lesson 8 Teaching Assistant: RoiYehoshua roiyeho@gmail.com

  2. Agenda • Running navigation stack in Gazebo with multiple robots • Sending goals to robots in Gazebo • Using amcl • Multi robot collision avoidance in navigation stack (C)2014 Roi Yehoshua

  3. Meet Lizi (C)2014 Roi Yehoshua

  4. lizi_description package (C)2014 Roi Yehoshua

  5. Setting Up Navigation Stack • Copy the package navigation_multiinto a new package called gazebo_navigation_multi • Change the name of the package in package.xml, CMakeLists.txt and move_base.xml • Remove stage_config subdirectory • Copy launch files (gazebo_multi.launch, robots.launch, one_robot.launch) from the launch directory in gazebo_multipackage • Change package name in launch files to gazebo_navigation_multi • Rename gazebo_multi.launch to navigation_multi.launch (C)2014 Roi Yehoshua

  6. gazebo_navigation_multi package (C)2014 Roi Yehoshua

  7. gazebo_navigation_multi launch files (C)2014 Roi Yehoshua

  8. Use Static Map • We will first run navigation stack with a static known map • Create a maps directory in your package and copy willow-full.png file into it • In navigation_multi.launch file make sure map_serverpoints to this map file (C)2014 Roi Yehoshua

  9. navigation_multi.launch <launch> <master auto="start"/> <param name="/use_sim_time" value="true"/> <node pkg="map_server" type="map_server" name="map_server" args="$(find gazebo_navigation_multi)/maps/willow-full.png 0.1" respawn="false" > <param name="frame_id" value="/map" /> </node> <!-- start gazebo --> <include file="$(find gazebo_navigation_multi)/launch/willowgarage_world.launch"/> <!-- include our robots --> <include file="$(find gazebo_navigation_multi)/launch/robots.launch"/> <node name="rviz" pkg="rviz" type="rviz" args="-d $(find gazebo_navigation_multi)/multi_robot.rviz" /> </launch> (C)2014 Roi Yehoshua

  10. robots.launch <launch> <!-- ROBOT 1 --> <group ns="lizi1"> <param name="tf_prefix" value="lizi1" /> <include file="$(find gazebo_navigation_multi)/launch/one_robot.launch" > <arg name="init_pose" value="-x 20 -y 15 -z 0" /> <arg name="robot_name" value="lizi1" /> </include> </group> <!-- ROBOT 2 --> <group ns="lizi2"> <param name="tf_prefix" value="lizi2" /> <include file="$(find gazebo_navigation_multi)/launch/one_robot.launch" > <arg name="init_pose" value="-x 22 -y 15 -z 0" /> <arg name="robot_name" value="lizi2" /> </include> </group> <!-- ROBOT 3 --> <group ns="lizi3"> <param name="tf_prefix" value="lizi3" /> <include file="$(find gazebo_navigation_multi)/launch/one_robot.launch" > <arg name="init_pose" value="-x 24 -y 15 -z 0" /> <arg name="robot_name" value="lizi3" /> </include> </group> </launch> (C)2014 Roi Yehoshua

  11. one_robot.launch • Copy navigation stack nodes (move_base, fake_localization) to one_robot.launch • Add a static transform publisher between base_link and laser_link frames (will be explained later) (C)2014 Roi Yehoshua

  12. one_robot.launch (1) <launch> <arg name="robot_name"/> <arg name="init_pose"/> <param name="robot_description" command="$(find xacro)/xacro.py $(find lizi_description)/urdf/lizi.urdf"/> <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="$(arginit_pose) -urdf -paramrobot_description -model $(argrobot_name)" output="screen"/> <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher"/> <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen"/> <!-- Run navigation stack --> <node pkg="move_base" type="move_base" respawn="false" name="move_base_node" output="screen"> <remap from="map" to="/map" /> <param name="controller_frequency" value="10.0" /> <rosparam file="$(find gazebo_navigation_multi)/move_base_config/costmap_common_params.yaml" command="load" ns="global_costmap" /> <rosparam file="$(find gazebo_navigation_multi)/move_base_config/costmap_common_params.yaml" command="load" ns="local_costmap" /> <rosparam file="$(find gazebo_navigation_multi)/move_base_config/local_costmap_params.yaml" command="load" /> <rosparam file="$(find gazebo_navigation_multi)/move_base_config/global_costmap_params.yaml" command="load" /> <rosparam file="$(find gazebo_navigation_multi)/move_base_config/base_local_planner_params.yaml" command="load" /> </node> (C)2014 Roi Yehoshua

  13. one_robot.launch (2) <node pkg="fake_localization" type="fake_localization" name="fake_localization" respawn="false" output="screen"> <param name="odom_frame_id" value="$(argrobot_name)/odom" /> <param name="base_frame_id" value="$(argrobot_name)/base_link" /> </node> <node pkg="tf" type="static_transform_publisher" name="laser_link_broadcaster" args="0 0 1 0 0 0 $(argrobot_name)/base_link $(argrobot_name)/laser_link 100" /> </launch> (C)2014 Roi Yehoshua

  14. TF Tree • For the navigation stack to work properly, the robot needs to publish the following transformations: map published by the localization system robotX/odom published by Gazebo’s driver controller robotX/base_link published by static tfdefined in your launch file robotX/laser_link (C)2014 Roi Yehoshua

  15. Gazebo’s Driver Controllers • http://gazebosim.org/wiki/Tutorials/1.9/ROS_Motor_and_Sensor_Plugins • The driver controller is specified in the robot’s URDF file • There are different Gazebo built-in driver plugins that you can use, including: • differential_drive_controller – a basic controller for differential drive robots (useful for robots with 2 wheels) • skid_steer_drive_controller – a basic controller for skid steering drive robots in Gazebo (useful for robots with 4 wheels, such as Pioneer 3AT) • The driver controller is automatically publishing the transformation from odom to base_link (C)2014 Roi Yehoshua

  16. Driver Plugin Definition <plugin name="skid_steer_drive_controller" filename="libgazebo_ros_skid_steer_drive.so"> <updateRate>100.0</updateRate> <leftFrontJoint>front_left_joint</leftFrontJoint> <rightFrontJoint>front_right_joint</rightFrontJoint> <leftRearJoint>rear_left_joint</leftRearJoint> <rightRearJoint>rear_right_joint</rightRearJoint> <wheelSeparation>0.255</wheelSeparation> <wheelDiameter>0.154</wheelDiameter> <odometryTopic>odom</odometryTopic> <odometryFrame>odom</odometryFrame> <robotBaseFrame>base_link</robotBaseFrame> <torque>500</torque> <commandTopic>cmd_vel</commandTopic> <broadcastTF>1</broadcastTF> </plugin> (C)2014 Roi Yehoshua

  17. Gazebo position_3d Controller • In addition, you need to make Gazebo publish the robot’s position to ROS • The easiest way to do this is with the position_3d controller • Add the following to your URDF file: <plugin name="p3d_base_controller" filename="libgazebo_ros_p3d.so"> <alwaysOn>true</alwaysOn> <updateRate>100.0</updateRate> <bodyName>base_link</bodyName> <topicName>base_pose_ground_truth</topicName> <gaussianNoise>0</gaussianNoise> <frameName>/map</frameName> <xyzOffsets>0 0 0</xyzOffsets> <rpyOffsets>0 0 0</rpyOffsets> </plugin> (C)2014 Roi Yehoshua

  18. Laser Scanner Controller • The gazebo model, written in URDF, makes use of an Hokuyo laser scanner through the libgazebo_ros_laser.so plugin • The frameName property of the plugin specifies the TF frame that corresponds to the laser link <plugin name="gazebo_ros_laser_controller" filename="libgazebo_ros_laser.so"> <alwaysOn>true</alwaysOn> <updateRate>40</updateRate> <topicName>base_scan</topicName> <frameName>laser_link</frameName> </plugin> (C)2014 Roi Yehoshua

  19. Laser Scanner Controller • However, the laser plugin doesn't automatically publishes a transform from base_link to the frame that you specified in the file (laser_link) • Thus we need to manually specify a static transform publisher between the laser_link and base_link frames • Add this publisher to one_robot.launch file (C)2014 Roi Yehoshua

  20. Laser Scanner Controller • <launch> • <arg name="robot_name"/> • <arg name="init_pose"/> • <param name="robot_description" command="$(find xacro)/xacro.py $(find lizi_description)/urdf/lizi.urdf"/> • <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="$(arginit_pose) -urdf -paramrobot_description -model $(argrobot_name)" output="screen"/> • <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher"/> • <node pkg="robot_state_publisher" type="robot_state_publisher" • name="robot_state_publisher" output="screen"/> • <!-- Run navigation stack --> • <node pkg="move_base" type="move_base" respawn="false" name="move_base_node" output="screen"> • … • </node> • <node pkg="fake_localization" type="fake_localization" name="fake_localization" respawn="false" output="screen"> • <param name="odom_frame_id" value="$(argrobot_name)/odom" /> • <param name="base_frame_id" value="$(argrobot_name)/base_link" /> • </node> • <node pkg="tf" type="static_transform_publisher" name="link_broadcaster" args="0 0 1 0 0 0 $(argrobot_name)/base_link $(argrobot_name)/laser_link 100" /> • </launch> (C)2014 Roi Yehoshua

  21. TF Tree • The TF tree should now look like this: (C)2014 Roi Yehoshua

  22. Map in Gazebo and rviz • By default the origin of the map is different in Gazebo and rviz • In Gazebo the origin is by default at the center of the map while in rviz it is at the lower-left corner • The map’s pose in Gazebo can be changed by adjusting its corresponding model in Gazebo’s world file • For that purpose, we first need to copy the world’s file into our package (C)2014 Roi Yehoshua

  23. Change Map’s Pose in Gazebo • Create worlds directory in your package • Copy willowgarage.world file from gazebo’s worlds directory to worlds directory of your package • Now edit the world’s file to adjust the model’s pose • The pose parameter consists of 6 values: <x y z r p y> • Angles are specified in degrees • $ roscdgazebo_navigation_multi/worlds • $ cp /usr/share/gazebo-1.9/worlds/willowgarage.world . (C)2014 Roi Yehoshua

  24. willowgarage.world • <?xml version="1.0" ?> • <sdf version="1.4"> • <world name="default"> • <include> • <uri>model://ground_plane</uri> • </include> • <include> • <uri>model://sun</uri> • </include> • <include> • <uri>model://willowgarage</uri> • <pose>-3 54 0 0 0 30</pose> • </include> • </world> • </sdf> (C)2014 Roi Yehoshua

  25. Change Map’s Pose in Gazebo • We also need to update the launch file of Gazebo’s world to launch our own version of the world file • First copy willowgarage_world.launch from gazebo_ros package to the launch directory of your package • Now edit this file • $ roscdgazebo_ros/launch • $ cp willowgarage_world.launch ~/catkin_ws/src/gazebo_navigation_multi/launch (C)2014 Roi Yehoshua

  26. willowgarage_world.launch • <launch> • <!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched --> • <include file="$(find gazebo_ros)/launch/empty_world.launch"> • <arg name="world_name" value="$(find gazebo_navigation_multi)/worlds/willowgarage.world"/> • <arg name="paused" value="false"/> • <arg name="use_sim_time" value="true"/> • <arg name="gui" value="true"/> • <arg name="headless" value="false"/> • <arg name="debug" value="false"/> • </include> • </launch> (C)2014 Roi Yehoshua

  27. Running the Navigation Stack • Now we are ready to run the navigation stack • $ roslaunchgazebo_navigation_multinavigation_multi.launch (C)2014 Roi Yehoshua

  28. Gazebo (C)2014 Roi Yehoshua

  29. rviz (C)2014 Roi Yehoshua

  30. Set Navigation Goal • Open rviz menu – Tool Properties • Change the topic name for the 2D Nav Goal according to the robot that you want to activate: (C)2014 Roi Yehoshua

  31. Set Navigation Goal (C)2014 Roi Yehoshua

  32. Following the Global Plan (C)2014 Roi Yehoshua

  33. Final Pose (C)2014 Roi Yehoshua

  34. Final Pose in Gazebo (C)2014 Roi Yehoshua

  35. Change Speed Limits • To change the speed limits of the robot used by the navigation stack, open the file move_base_config/base_local_planner_params.yaml • There are 4 parameters that determine the speed limits • max_vel_x: maximum linear velocity • min_vel_x: minimum linear velocity • max_rotational_vel: maximum angular velocity • min_in_place_rotational_vel: minimum angular velocity (C)2014 Roi Yehoshua

  36. Change Speed Limits • For the lizi robot we are going to change the following speed limits: • #Set the velocity limits of the robot • max_vel_x: 1.5 • min_vel_x: 0.5 • max_rotational_vel: 1.5 • min_in_place_rotational_vel: 1.0 (C)2014 Roi Yehoshua

  37. Sending Goals From Terminal • To send a goal to a robot from terminal, you can publish a message to the topic [robot_name]/move_base_simple/goal • For example to send a goal command to lizi2: • $ rostopicpub /lizi2/move_base_simple/goal geometry_msgs/PoseStamped '{header: {frame_id: "map"}, pose: {position: {x: 22, y: 17, z: 0}, orientation: {x: 0, y: 0, z: 0, w: 1}}}' (C)2014 Roi Yehoshua

  38. Sending Goals From Terminal (C)2014 Roi Yehoshua

  39. Sending Goals From Code • Copy send_goals.cpp from the package navigation_multi • Add send_goals.cpp to CMakeLists.txt • Change the name of the executable from send_goal to gazebo_send_goal • You cannot have two executables in the same workspace • Compile the package • Now you can send goals to robots by running the gazebo_send_goal node (C)2014 Roi Yehoshua

  40. Sending Goals From Code (C)2014 Roi Yehoshua

  41. AMCL • amcl is a probabilistic localization system for a robot moving in 2D • It implements the AdaptiveMonte Carlo localization approach, which uses a particle filter to track the pose of a robot against a known map • To use amcl, in one_robot.launch remove fake_localization node and include amcl_node.xml from move_base_config directory (C)2013 Roi Yehoshua

  42. AMCL • <launch> • … • <!-- Run navigation stack --> • <node pkg="move_base" type="move_base" respawn="false" name="move_base_node" output="screen"> • … • </node> • <!--<node pkg="fake_localization" type="fake_localization" name="fake_localization" respawn="false" output="screen"> • <param name="odom_frame_id" value="$(argrobot_name)/odom" /> • <param name="base_frame_id" value="$(argrobot_name)/base_link" /> • </node>--> • <include file="$(find gazebo_navigation_multi)/move_base_config/amcl_node.xml"/> • <node pkg="tf" type="static_transform_publisher" name="laser_link_broadcaster" args="0 0 1 0 0 0 $(argrobot_name)/base_link $(argrobot_name)/laser_link 100" /> • </launch> (C)2013 Roi Yehoshua

  43. AMCL • Localization made by AMCLneeds some help in providing the initial position of the robots on the map • For this purpose we will introduce 3 paramscarrying those values: initial_pose_x, initial_pose_y and initial_pose_a (their default value is 0) • We will define them in robots.launch file • Amcl node runs is in the robot namespace and thus the whole path of these params would be like /robotX/amcl/initial_pose_x (C)2013 Roi Yehoshua

  44. robots.launch • <launch> • <!-- ROBOT 1 --> • <group ns="lizi1"> • <param name="tf_prefix" value="lizi1" /> • <param name="amcl/initial_pose_x" value="20" /> • <param name="amcl/initial_pose_y" value="15" /> • <include file="$(find gazebo_navigation_multi)/launch/one_robot.launch" > • <arg name="init_pose" value="-x 20 -y 15 -z 0" /> • <arg name="robot_name" value="lizi1" /> • </include> • </group> • <!-- ROBOT 2 --> • … • </launch> (C)2013 Roi Yehoshua

  45. Set Initial Pose in rviz • You can also set the initial pose in rviz • First open Tool Properties and change the 2D Pose Estimate topic to the specific robot’s initialpose topic • Then you can use the 2D Pose Estimate button to specify the initial pose of the robot (C)2013 Roi Yehoshua

  46. Multi Robot Collision Avoidance • http://wiki.ros.org/multi_robot_collision_avoidance • This package includes a patch to AMCL that can take into account other robots navigation paths • The AMCL patch is necessary to publish a Weighted Point Cloud needed for the calculation • Multi Robot Collision Avoidance Demo • No version of this package for ROS Hydro yet  • Opportunity to make some contribution to the community! (C)2014 Roi Yehoshua

  47. Homework (not for submission) • Spawn 3 Pioneer robots in Gazebo • Add all the necessary Gazebo controllers and definitions to the robot’s URDF file • Run ROS navigation stack on these robots (without amcl and with amcl) (C)2014 Roi Yehoshua

More Related