pipeline_planner ROS Package

summary

pipeline_planner is a kind of global planner. It is designed to be used for a robot, especially nonholonomic robot, in a warehouse. We assume a robot to follow a fixed route which is determined in advance for the warehouse. We construct the fixed route by a series of checkpoints. Connected checkpoints and radii of pipeline segments make “pipeline”. pipeline_planner makes a global path in the pipeline. For making the path, the planner veils the area out of the pipeline and call the global planner NavFn on the edited global map. So we need NavFn planner for dependency. We explain them in detail in the next section.

This planner is described for kinetic version of ROS now.

function

We can construct a pipeline by a series of checkpoints, an open close flag and radii for pipeline segments. We cast these information by the service server “receive_checkpoints”. Each pipeline segment is constructed by connecting sequential checkpoints together with the pipeline segment radius for it. We can set the pipeline radii for pipeline segments as a positive value from 0.001 to 50 metre. If we set values for some pipeline segments as 0.0, the radii uses a default value. The default value is indicated by the dynamic parameter “pipe_radius”. If we change the value “pipe_radius”, then radii for default set pipeline segments will change. When we want to change a radius of a pipline segment, we can do it by calling the “set_a_radius” service. A pipeline has a direction and a robot can’t go backwards in the pipeline. A series of checkpoints is an ordered points in a global map. It must be in the global map. It mustn’t have a crossing after these checkpoints are connected to be a route. A pipeline might be an open route or a close route. Probably we use an open route as a one time only route because a robot can’t go backwards in the route.

When we cast checkpoints with an open close flag, there might be an error. So the planner has the status for reading checkpoints. The status for reading checkpoints is immediately published on the topic “read_checkpoints” when the planner receives the checkpoints. And we can inquire the state at any time by the service “get_read_status”. We can use the planner when the value of the state is 3 or 5. The list of the status will be described later.

After the planner receives proper checkpoints and open close flag, a robot is ready to move. When the planner receives a global planning order from move_base, it is, move_base calls makePlan function of the global planner, the planner veils the costmap with “lethal_cost” other than that in the pipeline and it makes a global path by calling the NavFn planner. We can set “lethal_cost” by the parameter of the same name. It is not expected for a robot to move in one pipeline segment, it is, a region between two neighbouring checkpoints. We can avoid the situation by making enough checkpoints. We can adjust the parameter “centre_weight” so as a robot to go in the middle of a pipeline. If we want a robot to go in a little right hand side or in a little left hand side from the middle of a pipeline segment, then we can set it by the poses.orientation.y variables of a request in receive_checkpoints service. We can change the right shift or left shift parameter by the service “set_a_rightshift”. A robot might stop by an error. We can gain the state of the robot by calling the service “get_robot_status”. 2 or 3 is the welcome value. The list of the robot status will be described later.

When makePlan is called start of calculation is informed by the topic “inform_status” with the value 12. If the calculation completed normally, the end of calculation is informed by the same topic with the value 3. In a case of an error, the termination of the calculation is informed by the same topic with the robot status value (again, it will be described later).

When makePlan calculation takes too much time, we can use torch model. With the torch model, we can restrict the manipulation region, id est, the region in which we veil a cost or add a cost. We can change the torch area by dynamic parameters, “torch_area_x” and “torch_area_y”.

Two main functions are PipelinePlanner::initialize and PipelinePlanner::makePlan. ‘initialize’ is called when move_base is launched or pipeline_planner is set to move_base’s global planner dynamically. ‘makePlan’ is called when global planner is called from move_base. This system uses nav_core as a interface with move_base. These are common for any global planner.

pipeline_planner can use CUDA for GPU calculation. If we use CUDA, we need CUDA for dependency. How to use CUDA is described in the Usage chaper.

specification

The list for the status of reading checkpoints. (unsigned int)

1. not yet read
2. reading now
3. OK
4. read but empty
5. route overlaps obstacle (permitted)
6. a part of the pipeline is out of costmap
7. openclose flag error
8. received pipeline has a crossing
9. the radius of a pipe segment is not valid
10. some sequential checkpoints are too close
    others. unknown error

If a status is one other than 3 or 5, it is error status.

The list for the status of robot after makePlan was called. (unsigned int)

1. global planner is not yet initialised
2. the robot is ready to receive makePlan
3. global plan was made normally
4. the start is out of pipeline
5. all pipeline segments include start point
6. the goal is out of pipeline
7. all pipeline segments include goal point
8. goal is behind start (in the case of open pipeline only)
9. invalid check points (check “get_read_status”)
10. global planner fails to make a plan
11. goal is behind start in one pipe segment
12. now calculating global path
13. calculation of local global goal fails, probably torch area should be widen

If the status is one other than 2 or 3, it is error status.

About checkpoints. The input format for checkpoints is geometry_msgs::PoseArray. In the format, we use “header.frame_id”, “poses.position.x”, “poses.position.y”, “poses.position.z” and “pose.orientation.y”. We can set a pipeline as a close loop or a open loop. Probably you use an open loop as an one time only loop.

“header.frame_id”: The flag for whether the pipeline is close(value is 1) or open(value is 2).
“poses.position.x” and “poses.position.y”: The position of checkpoints.
“poses.position.z”: The radius of the pipeline segment. If you set the value as 0.0, then it uses the dynamic parameter value “pipe_radius”. You can change the value by the service “set_a_radius”.
“pose.orientation.y”: The deviation from the middle of the pipeline segment into right hand side. If you want a robot to go 0.5 metre right hand side from the middle of the pipeline segment in the pipeline segment, you should set the value as 0.5. If you want left hand side, you should use a negative value. You can change the value by the service “set_a_rightshift”.

• Subscriptions:

• /move_base/goal
  type: move_base_msgs::MoveBaseActionGoal

• /robot_pose
  type: geometry_msgs::Pose

• Publications:

• ~/<name>/read_checkpoints
  type: std_msgs::UInt32
  description: The result of checkpoints subscription in the reading checkpoints status form.

• ~/<name>/inform_status
  type: std_msgs::UInt32
  description: The result of robot status.

• ~/<name>/plan
  type: nav_msgs::Path
  description: Global path especially for rviz visualisation.

• ~/<name>/robot_position
  type: pipeline_planner::RobotPosition
  description: To inform robot position of a pipe segment in the form.
  • in: Whether the robot is in the pipeline or not.
  • id: The ID of pipeline segment in which the robot currently is, which starts from 0. If the robot is out of pipeline, then the value will be -1.
  • previd: The previous ID of “id”.
  • nextid: The next ID of “id”.
  • startdist: The distance from start position of the pipeline.
  • enddist: The distance from end position of the pipeline.

• ~/<name>/initialised
  type: std_msgs::Bool
  description: Notification of the end of initialisation. The value is always true.

• ~/<name>/time
  type: std_msgs::Float32
  description: The consumed time of makePlan function.

• ~/<name>/visualise_pipeline
  type: nav_msgs::OccupancyGrid
  description: To visualise pipeline on rviz.

• Services:

• ~/<name>/get_read_status
  INPUT: none
  OUTPUT: uint32
  description: To inform the status of the read checkpoints

• ~/<name>/get_robot_status
  INPUT: none
  OUTPUT: uint32
  description: To inform the status of the robot

• ~/<name>/get_checkpoints
  INPUT: none
  OUTPUT: geometry_msgs/PoseStamped[], uint32
  description: To inform the checkpoints

• ~/<name>/get_numof_checkpoints
  INPUT: none
  OUTPUT: uint32
  description: To inform the number of the checkpoints

• ~/<name>/inquire_segments
  INPUT: none
  OUTPUT:
  • int32 num_segments (the number of segment)
  • SegmentInfo[] sinfo (each segment’s information)
    SegmentInfo:
  • int32 ID (current segment ID)
  • int32 prevID (previous segment ID)
  • int32 nextID (next segment ID)
  • float64 length (the length of the segment)
  • bool start (start flag for robot passing)
  • bool end (end flag for robot passing)
    description: To inquire pipe segment information. If the robot is in the n-th pipe segment, then start flag of n-th segment and end flag of (n-1)-th segment is true. Other flags are false.
• ~/<name>/set_a_radius
  INPUT: int32 ID, float64 radius
  OUTPUT: int32 result, uint32 read_status
  result values
  • 0: accepted normally
  • 1: ID value is not -1, 0 nor positive
  • 2: The value is illegal, id est, other than 0.0 or [0.001,50].
  • 3: A set of checkpoints is empty
  • 4: ID is larger than the number of checkpoints description: To change the radius of the pipeline segment. The pipeline segment is indicated by the ID and it starts from 0. If we set ID as -1, the radii of all pipeline segment will change. The new radius we want to set is indicated by “radius”. If we set “radius” as 0.0, then we use default value, id est, the same value as “pipe_radius” in dynamic reconfigure. We will receive Updated “read_status” as an output.
• ~/<name>/set_a_rightshift
  INPUT: int32 ID, float64 shift
  OUTPUT: int32 result
  result values
  • 0: accepted normally
  • 1: ID value is not -1, 0 nor positive
  • 3: A set of checkpoints is empty
  • 4: ID is larger than the number of checkpoints description: To change the value for right shift from the middle of the pipeline segment. The pipeline segment is indicated by the ID and it starts from 0. If we set ID as -1, the radii of all pipeline segment will change. The new right value for the pipeline segment is indicated by the “shift”. If the value for shift is larger than the value for “pipe_radius”, the robot tends to go at the right edge of the pipeline or the left edge of that according to whether the value is positive or negative.

• ~/<name>/receive_checkpoints
  INPUT: geometry_msgs/PoseArray, checkpoints
  OUTPUT: geometry_msgs/PoseStamped[], uint32
  description: To receive checkpoints. The output value is the status of reading checkpoints.

• Parameters:

• Dynamic reconfigure:
• pipe_radius (double): radius of pipeline
• lethal_cost (int): Lethal Cost (253)
• centre_weight (double): tendency for a robot to go in the middle of a pipe line
• num_threads (int): The number of thread to use in CUDA mode. If you set the value too large, the value is automatically amended.
• use_straight (bool): Whether the robot uses straight line mode, it is, making a plan by connecting checkpoints with straight line segments instead of veiling out side of pipe line
• charge (bool): To make a straight route without care of cost in use_straight mode
• time_display (bool): To display time measuring information on terminal
• use_torch (bool): Whether we use torch model or not.
• torch_area_x (double): width (x) of bright area, metre
• torch_area_y (double): depth (y) of bright area, metre

###Dependency We need navfn package for dependency. And we also need CUDA for GPU calculation. However if you don’t implement GPU calculation by a setting, CUDA is not required.

###Usage You can use the planner with these steps.

1. Make pipeline_planner by catkin_make. NavFn is required for its dependency.

2. Edit move_base.launch.xml file or move_base launch file like below.

  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <param name="base_global_planner" value="pipeline_planner/PipelinePlanner"/>

1. Edit move_base_params.yaml file as below.

#base_global_planner: "navfn/NavfnROS"
base_global_planner: "pipeline_planner/PipelinePlanner"

1. Launch move_base.

2. Cast checkpoints by service. Data type is geometry_msgs::PoseArray.

3. Navigate a robot from move_base.

pipeline_planner can use CUDA for GPU calculation. If you want to use CUDA, you can do it by the setting below.

Change these two lines in the file CMakeLists.txt
#set(USE_GPU TRUE)
set(USE_GPU FALSE)
into the below.
set(USE_GPU TRUE)
#set(USE_GPU FALSE)

###flow chart

Licence

The *pipeline_planner package is licensed under the BSD license.

Changelog for package global_planner

0.1.3.4 (1.6.4) (2019-07-04) -------------------* Bugfix for checkpoints crossing validity.

0.1.3.3 (1.6.3) (2019-06-24) -------------------* Deletion of fix_pipe_radius parameter. * Addition of set_a_radius service. * Addition of set_a_rightshift service.

0.1.3.2 (1.6.2) (2019-06-17) -------------------* Addition of pipeline visualisation function for rviz. * Addition of a code for a robot to run in the right hand side from the pipeline segment centre.

0.1.3.1 (1.6.1) (2019-05-29) -------------------* Addition of latch flag on initialised publication.

0.1.3.0 (1.6.0) (2019-05-28) -------------------* Addition of initialisation notification as a topic * Addition of reception of checkpoints as a service

0.1.2.5 (1.5.5) (2019-05-10) -------------------* Change of output format for robot_position topic. * Addition of inquire_segments service.

0.1.2.4 (1.5.4) (2019-04-22) -------------------* Modification of main function for melodic version

0.1.2.3 (1.5.3) (2019-04-19) -------------------* Modification of torch model

0.1.2.2 (1.5.2) (2019-04-17) -------------------* Bug fix about back veil in order not to go back

0.1.2.1 (1.5.1) (2019-03-28) -------------------* Slight bug fix for checkpoints reception * Slight change of specification for robot_position publication * Addition of "get_numof_checkpoints" service

0.1.2.0 (1.5.0) (2019-03-25) -------------------* Addition of informing robot position function

0.1.1.1 (1.4.1) (2019-03-25) -------------------* Bug fix for unexpected short cut

0.1.1.0 (1.4.0) (2019-03-19) -------------------* Addition of torch model * Making thread number dynamic * Addition of thread number checking issue: dynamic parameter for "num_threads" doesn't change * Making DistanceFromCentre function CUDA based

0.1.0.2 (1.3.3) (2019-03-13) -------------------* Slight change about costmap for fast calculation

0.1.0.1 (1.3.2) (2019-03-13) -------------------* Modification of consuming time and addition of time consuming publishing on topic. * Slight bug fix for SubscribeCheckpoints function.

0.1.0.0 (1.3.1) (2019-02-28) -------------------* Addition of charge param.

1.3.0 (2019-02-13)

• Addition of CUDA calculation function.

1.2.0 (2019-02-12)

• Addition of informing start and end of makePlan to tablet by a topic.

1.1.0 (2019-02-08)

• Addition of not fix radius mode.
• Addition of use straight line mode.

1.0.0 (2018-12-12)

• Change of README.md for public release.

0.1.4 (2018-07-26)

* Addition of centre_weight parameter. A robot goes in the middle with the

parameter.

• Replacement from global_planner into navfn for base planner.

0.1.3 (2018-06-27)

• Addition of getCheckpoints as a service

0.1.2 (2018-06-13)

• getReadStatus and getRobotStatus as a service
• Change of the default value of pipe_radius
• Addition of the crossing check routine for pipeline

0.1.1 (2018-03-27)

• Addition of getStatus function as a service server

0.1.0 (2018-03-26)

• Initial commit