Robots modelling, localization, mapping and navigation
Udacity Project6 “Home Service Robot”
Course Project from Robotics Software Engineer – Path planning and navigation.
localization system for a robot should be installed: sudo apt-get install ros-melodic-navigation
Description
Scripts
- test_slam.sh
- test_navigation.sh
- pick_objects.sh
- add_marker.sh
- home_service.sh
test_slam.sh
Script does the following:
- Launches world.launch (from package my_robot).With the help of this launch-file an environment starts, which I create with gazebo. This is a model of my apartment (wohnung.world). Moreover my robot is loaded. I create the robot in Unified Robotic Description Format (URDF) (my_robot.xacro). Nodes, which publish state of robot and joint states are also launched. Finally rviz, a 3D visualization tool for ROS, starts.
- Launches slam_gmapping_pr2.launch (from package slam_gmapping).With the help of launch configuration file node slam_gmapping is run. The node uses OpenSlam and provides laser-based SLAM. With usage of slam_gmapping, I create a 2-D occupancy grid map.
- Launches teleop_twist_keyboard.py (from package teleop_twist_keyboard).With this node I drive a robot in my environment, in order to gather information about obstacles (mostly walls) in my apartment.
This script is used to create an environment map, which I will use further in navigation. I create the bitmap representation of map with the help of map_server. The information, which was gathered by slam_gmapping, is provided by topic map (nav_msgs/OccupancyGrid) to map_server. Node map_server creates a bitmap representation and yaml file, which describes parameters of map. Map (wohnung.pgm and wohnung.yaml) is saved in ./my_robot/map/
test_navigation.sh
Script does the following:
- Launches world.launch (from package my_robot).See description from test_slam.sh
- Launches amcl.launch.The file starts map_server node, amcl from package navigation node, move_base from navigation package node. Node amcl realizes probabilistic localization of a robot in 2D. To track the pose of a robot against a known map, it uses Adaptive (or KLD-sampling) Monte Carlo algorithm. move_base is a node which controls the robot movements, so that it can reaches the goal (so the goal should be provided). It uses components global_planner, global_costmap, local_planner, local_costmap, recovery_behaviours for creating paths.
- Runs map_server node, which provides a map to move_base.
The script is used to manually set goal and navigate to it.
pick_objects.sh
Script does the same as test_navigation.sh. In addition, it runs node pick_objects_node from package pick_objects with command line argument 0 (argument 0 means don’t use add_markers_node). Node pick_objects_node constructs an object of SimpleActionClient, with help of which the node can communicate with robot’s action. To this object the node sends a goal. After reaching the first goal, the node waits for 5 seconds. Afterwords, the node sends the second goal to SimpleActionClient object and waits for the robot reaches the goal.
add_marker.sh
Script does the same as test_navigation.sh. In addition, I run node add_markers_node from package add_markers with command line -9.0 -5.0 -4.0 3.0. Through command line I pass positions of two goals (x1, y1, x2, y2). The node analyzes command line arguments, set marker properties and publishes topic visualization_marker to rviz.
home_service.sh
Script does the same as test_navigation.sh. In addition, I run the nodes, which I create: pick_objects_node and add_markers_node. They are started without arguments. It means that they communicate with each other. Node pick_objects_node sends two navigation goals to move_base node via SimpleActionClient. After the first goal is sent to move_base node, node pick_objects_node sends request to add_markers_node node. The request asks add_markers_node to show the marker on the place of first goal. Node pick_objects_node waits until the robot reaches the first goal. Afterwards pick_objects_node sends request to delete marker (node add_markers_node handles this request) at the place of first node. Node pick_objects_node makes the robot to wait 5 seconds. Afterwards the second goal is sent to move_base by node pick_objects_node. When second goal is reached, pick_objects_node sends request to add_markers_node to show marker at the place of second goal.
Significant nodes
slam_gmapping
This node implements OpenSlam’s Gmapping with usage of Rao-Blackwellized particle filter. I use the node to build the map, with help of laser and pose data of the robot (the node subscribes to tf and scan topics). The 2-D occupancy grid map I retrieve with help of map_server node and map topic.
amcl
amcl node provides probabilistic localization in 2D. To track the robot’s pose inside known map, which I get with a help of gmapping node, the amcl node implements the adaptive (or KLD-sampling) Monte Carlo localization approach. Inputs for the node are a laser-based map, laser scans, and transform messages. Output is pose estimation.
move_base
The node move_base creates an action for robot to achieve the given goal. Inputs for the node are the following:
- tf/Message from amcl node;
- nav_msgs/Odometry from amcl node;
- geometry_msgs/PoseStamped from rviz or pick_objects nodes;
- nav_msgs/GetMap (navigation stack setup) from map_server node;
- sensor_msgs/LaserScan from gazebo node;
The output of the node is cmd_vel topic, which is aimed for base controller.
Node move_base consists of the following components:
- global_planner implements fast, interpolated global planner for navigation. Global Dynamic Window Approach is used;
- local_planner implements Trajectory Rollout and Dynamic Window for local robot navigation on a plane. Inputs of component are plan to follow and a costmap. The output is velocity commands for a mobile base;
- global_costmap is used for creating long-term plans. Stores information about obstacles;
- local_costmap is used for local planning and obstacle avoidance. Stores information about obstacles;
pickup_objects_node
I create this node in order to sequentially send two goals. Node pickup_objects_node has two modes:
- Work without add_markers_node node. See description of this mode in pick_objects.sh section.
- Work with add_markers_node. The service requests are sent to the add_markers_node to show or delete markers at pickup or drop off goals. To run without communication with add_markers_node node.
rosrun pick_objects pick_objects_node 0
To run with communication with add_markers_node node.
rosrun pick_objects pick_objects_node [1]
add markers_node
I create this node to add markers. Node add_markers_node has two modes:
- Work without communication with pickup_objects_node node. See description of this mode in add_marker.sh section.
- Work with communication with pickup_objects_node node. In this case add_marker_node node handles requests to add/delete markers at definite places. These requests are sent by _pickup_objects_node node.
According to the task the following protocol is implemented:
- Initially show the marker at the pickup zone.
- Hide the marker once my robot reach the pickup zone.
- Wait 5 seconds to simulate a pickup.
- Show the marker at the drop off zone once my robot reaches it.
To run without communication with pick_objects_node node.
rosrun add_markers add_markers_node x1 y1 x2 y2
x1, y1, x2, y2 – floating point coordinates of marker.
To run with communication with pick_objects_node node.
rosrun add_markers add_markers_node
rviz
This is a visualization tool. The most interesting topics, to which rviz subscribes:
- visualization_marker is published by add_makers_node node to show marker;
- /move_base/NavfnROS/plan is published by move_base node. It is a trajectory path, which is built by global planner, to reach a goal;
- /map is published by map_server node;
- /move_base/global_costmap/costmap is published by move_base node is very useful to see global costmap;
gazebo
The most interesting topics (publish/subcribed) :
- /cmd_vel is published by move_base node to make robot drive;
- /my_robot/laser/scan, /odom, /tf are published by gazebo to amcl and gmapping nodes (slam and navigation);
Plugins libgazebo_ros_diff_drive.so, libgazebo_ros_laser.so are used for laser and robot movement features.
Description
Udacity
02-05/2020
Create a model of the world (gazebo), model of robot (urdf). Develop a ROS program to locate and navigate the robot (amcl, gmapping, move_base).