CSE 468/568: Robotic Algorithms A* Planning and Execution VERSION 1 PA2, Fall 2019 DEADLINE: October 17, 2019 11:59 pm INSTRUCTOR: Vivek Thangavelu WRITTEN BY: Vivek Thangavelu and Zijian An I. Objective The objective of this assignment is to autonomously plan and execute a path for a robot in the Stage simulator from a start location to a goal location, given a map. The global plan is given by A* and the local planning is done using a modified Vector Field Histogram (VFH) (Refer Section IV.6). II. Description 1. Supplied Files The ros_pa2.tar.gz compressed file contains three files: ● map.txt – The map of the simulator world described as a 1D array with 0s and 1s ● playground.pgm - Bitmap image used to describe the obstacles and free space for the stage simulator ● playground.world – Configuration file for the stage simulator that defines the robot and its environment (using the playground.pgm file) 2. Deliverables ● You will write node(s) to implement the VFH and A* algorithms. The default start location is (-8.0, -2.0) and the default goal location is (4.5, 9.0) . The goal should be defined as two ROS global parameters, ‘goalx’ and ‘goaly’, both of which should be of type double. This allows us to set a new goal position through the ROS parameters before running the nodes, and the robot will then plan a path to the new goal. Please go through the tutorial on ROS Parameters ● The final package should contain a pa2.launch file that should bring up the stage simulator (using the given world file) and your nodes. III. Initial Setup 1. Download and extract the supplied files for PA2 (SHA256: 7a7c43abe7842faa43873568b0979d4c0b6f08b86c1bcef7768168537e06abe6) 2. Create a new package called ros_pa2 cd ~/catkin_ws/src catkin_create_pkg ros_pa2 std_msgs geometry_msgs rospy roscpp cd ~/catkin_ws catkin_make source ~/.bashrc 3. Place the world files (playground.pgm and playground.world) into an appropriate subfolder within ros_pa2. (Similar to the ros_pa1 package) 4. Create a launch file called pa2.launch in ~/catkin_ws/src/ros_pa2 to run the stage simulator using the given world file. You can copy the launch file from PA1 and edit it accordingly. IV. Concepts 1. Simulator Stage is a 2(.5)D robotics standalone simulator. It provides a virtual world populated by mobile robots and sensors, along with various objects for the robots to sense and manipulate. The stageros node wraps the Stage 2-D multi-robot simulator, via libstage for ROS. Stage simulates a world as defined in the playground.world file, present inside the world sub-folder. This file tells stage everything about the world, from obstacles (usually represented via a bitmap to be used as a kind of background), to robots and other objects. You can change the view in the simulator and move the robot in stageros. You can add some basic visualizations such as the laser scanner range data or the robot footprints for debugging from the View menu in stageros. 2. Robot Motion The robot in stageros simulates a 2 wheeled differential drive robot that drives over a smooth ground plane. In order to move the robot in the simulator, we need to send velocity commands to the robot. This is done by publishing command geometry_msgs/Twist messages to the topic /cmd_vel. The robot can only respond to linear velocities in the x axis and angular velocities in the z axis. So, to move forward, publish Twist messages with non-zero linear.x velocities and to turn, publish Twist messages with non-zero angular.z values. A simple behaviour to move your robot without using the more complicated differential model is to always have only an angular or linear velocity at a specific time to turn or move forward, respectively. To know the pose of the robot, you can subscribe to /base_pose_ground_truth of type nav_msgs/Odometry and retrieve the position and orientation from the pose member variable. Orientations are expressed as quaternions in ROS. Please refer to section VII.3 for conversion between quaternion and euler angles. 3. Sensor In order to make sense of its environment, the robot needs to get data from its sensors. The robot is equipped with a planar laser range-finder that gives information about obstacles around the robot. The sensor has 361 lines of lasers allowing the robot to perceive 180 degrees of the world around it at a maximum range of 3m. In your stageros simulator, you can view the robot sensing range by clicking on View -> Data or pressing the key ‘D’. The sensor data is published to a topic /base_scan which is of type sensor_msgs/LaserScan. Use the ranges[i] member variable to get the range information of a particular laser line i. If there is an obstacle between range_min and range_max, range will be the distance of the sensed obstacle from the sensor. If an obstacle is outside its range values, the range will be equal to range_max. Move the robot around by clicking and dragging it in the simulator and visualize the output of the sensor in rviz to understand how the sensor works. Subscribe to the topic and read the messages to get the values of range_max, range_min, each range data, etc. 4. Map Global planners require a map for operation. The map of the robot workspace in the simulator is given as an occupancy grid, a grid representation with 1s and 0s, with 1 indicating an obstacle in that cell and 0 representing an empty cell. The map has 20 rows and 18 columns. The map is described as a 1D array with 0s and 1s in map.txt. You should include this into your program as the map. You can simply paste the array into your code and read it appropriately as a 2D matrix with dimensions of (20, 18). 5. Global planner (A* Planning): A* is an informed search technique used to search for a path from a start location to a goal location in a map. The total cost of a node n is given by: f(n) = g(n) + ε.h(n) where g(n) is the exact cost of the path from the start node to the node n and h(n) is the heuristic function which estimates a cost from node n to the goal node. Figure 1 depicts the movement cost from a node n (shown in green) in the occupancy grid to its 8 neighbours (shown in blue). The movement costs to the non-diagonal neighbours is 1 and the movement costs to the diagonal neighbours is √2 = 1.4. Using these movement costs and keeping track of the path taken, the exact cost of the path from the start to any node n in the occupancy grid can be calculated by the sum of the movement costs along the path taken. 1.4 1 1.4 1 0 1 1.4 1 1.4 Fig.1: Movement Cost Given the current node and the goal node, you can use the Euclidean distance (L2 Norm) between them as the heuristic cost. The A* algorithm essentials outputs a global path from the start node to the goal node. The global path essentially contains a list of nodes/checkpoints, such that when the robot moves to each one of them in succession it will eventually reach the final goal location. ε is the coefficient of the heuristic function, used to scale the heuristic cost. You can start with ε=1, and tune only if required, based on the paths returned by your A* algorithm. You may refer to this video for a walkthrough of the A* algorithm. 6. Local planner/Obstacle Avoidance (VFH): The precision in the map may not capture all the minute details in the simulator. Blindly following the path outputted by the global planner may lead to your robot crashing into obstacles. The local planner utilizes sensor data for obstacle avoidance to travel to each checkpoint in the global path, and eventually reach the final goal location. Fig.2 Polar Histogram You will
need to implement Vector Field Histogram (VFH) for obstacle avoidance. To make things simple, we skip the first step of deriving a local map around the robot based on recent sensor readings. Instead, the sensor data from topic /base_scan is directly used to construct the polar histogram. The steps involved in the modified VFH are as follows: 1. Convert the sensor data into a polar histogram by grouping the sensor data at different angles into bins of specific size (a bin size of 5 might be a good start), similar to the polar histogram shown in Fig. 2. a. x axis – angle α at which the obstacle is found b. y axis – count of obstacles in that bin c. choose a reasonable threshold to filter out noisy sensor data (i.e a bin with a count less than the threshold may be treated as a bin with no obstacles) 2. All openings large enough for the robot to pass are found (this can be approximated based on your interactions with the simulator, considering the sensor range and the robot size specified in the world file) 3. Compute cost function G for all openings: G = a * target_direction + b * current_direction + c * previous_direction where target_direction is the difference between the chosen direction and the goal direction, current_direction is the difference between the chosen direction and the current heading direction, and previous_direction is the difference between the previously selected direction and the chosen direction. The coefficients a, b and c tune the behavior of the robot. 4. Opening with the lowest cost function G is chosen NOTE: You will need to figure out the values of the coefficients and histogram bin sizes that works best for your specific implementation. You may need to add some tweaks to the VFH algorithm for obstacle avoidance, as you did in PA1 for BUG2 with interim behaviours. This might be required because in both PA1 and PA2, we do not explicitly model the dynamics of the robot system. 7. Translation between Simulator and Map Given a position in the simulator, you will need to devise a method to find the position (grid index) in the map. This would be required for translating the start and goal locations from the continuous, simulator world to the discrete map representation. Similarly, a position (grid index) in the map needs to be translated to a position in the simulator world. This would be required for translating the nodes/checkpoints in the global path specified in the map representation, to the simulator world coordinates. These calculations can be derived by knowing the limits of the simulator world and the map. 8. Coordinate Frames In this robot system, there are 4 coordinate frames of reference. For this PA, we only care about the following aspects of the various frames of reference: ● The absolute coordinate frame of reference is named odom i.e. the root of the tree data structure that represents the various frames. ● The robot’s frame of reference is the base_link frame ● The robot pose is expressed in the odom frame ● The goal position is expressed in the odom frame You do not have to worry about the other frames that exist in the system as all the pose information is expressed in the same frame (odom) and the distances of the obstacles are measured from the robot’s frame. V. Implementation Tips Implementation in robotics can be a daunting task with multiple sub-objectives in robot control, namely local planning and global planning. It is always a good idea to list out the sub-objectives for your task and modularize your code accordingly. Below is a set of sub-objectives that might help you organize your work for this PA. You do not have to follow this strictly and is presented here purely as an example. 1. Read the map.txt as a 2D array 2. Write modules to translate between the simulator and the map 3. Based on the map, implement A* algorithm to extract a global path between the start and goal nodes 4. Convert sensor data into a polar histogram of a specific bin size 5. Implement the modified VFH algorithm 6. Given the global path as a list of checkpoint positions in the simulator world, move the robot using the VFH algorithm towards the next closest checkpoint 7. Repeat the above step for every checkpoint until you reach the final goal location 8. Read the goal location in your node as ROS parameters VI. Submission Instructions We will be using autolab in this course to submit your programming assignments. In the autolab course page, select pa2 and then submit a tarball of your entire ros_pa2 folder. From the file viewer, you can right click ros_pa2 and click compress to get a tarball of the entire folder. The deadline for submission is October 17, 2019 11:59 pm. The deadline will be strictly enforced so please submit once much earlier to test out the system. You are allowed to make multiple submissions. We will use the final submission version to grade your programming assignment. Late Submission Policy You may choose to submit the assignment late by a maximum of 2 days. Each day you lose 25% of the full grade. Hence, by submitting one day late, you lose 25% and by day 2, you lose 50%. The deadline for each extra day is 11:59 pm. VII. FAQ 1. What will I learn from this PA? At the end of the PA2, you will have learned: a. How to make your own ROS package and use ROS parameters b. How to perform an informed search technique to extract optimal/close-to-optimal global paths for a robot, given a map c. How to implement a local planner for the robot to execute the global path 2. How to read map.txt as a 2D array? a. You can copy the array values from map.txt and paste it into your python script/c++ source file b. You can use the grid size mentioned in Section IV.4 to convert the 1d representation into a 2d array (For python, you may use numpy to restructure the 1d array into a 2d array) 3. How to transform from quaternion to euler? ROS uses quaternions to track and apply rotations. A quaternion has 4 components (x,y,z,w). A quaternion is an another way to express a 3d rotation in space. You can convert a 3d orientation from quaternion to euler angles and vice versa. You may refer to the following links for example code: For Python: https://answers.ros.org/question/69754/quaternion-transformations-in-python/ For C++: https://gist.github.com/marcoarruda/f931232fe3490b7fa20dbb38da1195ac 4. How to import/include a message module? Every message is defined inside a package. So you must first know the name of the package and the message type. For example, /cmd_vel is of type geometry_msgs/Twist. geometry_msgs is the name of the package and Twist is the type of message. To import in Python: from .msg import To include in C++: #include “/.h” 5. rosrun does not run my node Python: a. Make sure your python script is in the scripts folder of your ROS package b. Make sure you have given executable privileges to your python script: chmod +x .py c. Add the python header to your script: #!/usr/bin/env python C++: a. Make the necessary changes to the CMakeLists.txt file inside your package folder b. Make sure you ran catkin_make and sourced your bashrc file: catkin_make source ~/.bashrc VIII. References: 1. ROS Package: http://wiki.ros.org/ROS/Tutorials/CreatingPackage 2. ROS Parameter: http://wiki.ros.org/Parameter%20Server 3. Stage 4.1.1 Manual: https://codedocs.xyz/CodeFinder2/Stage/md_README.html 4. stageros Wiki: http://wiki.ros.org/stage_ros 5. A* Algorithm Walkthrough: https://www.youtube.com/watch?v=-L-WgKMFuhE 6. Histogram: https://en.wikipedia.org/wiki/Histogram 7. Autolab: https://autograder.cse.buffalo.edu/courses/CSE468-f19/assessments 8. VFH MATLAB: https://www.mathworks.com/help/nav/ug/vector-field-histograms.html
