search

cogsys-tuebingen / gerona

GeRoNa (Generic Robot Navigation) is a modular robot navigation framework, that bundles path planning and path following (including obstacle detection) and manages communication between the individual modules. It is designed to be easily extensible for new tasks and robot models.
http://www.ra.cs.uni-tuebingen.de/forschung/gerona/
BSD 3-Clause "New" or "Revised" License
325 stars 126 forks source link

Help using the package with Hector SLAM #22

Closed michaelchi08 closed 4 years ago

michaelchi08 commented 4 years ago

Hi, I'm trying to use gerona with hector slam in real time. The idea is is to use the 2d nav goal in rviz to provide waypoints in an unknown map to move the robot while hector slam maps the surrounding and provides localization. Is that approach possible? Any advises would be welcome.

Thanks so much, Michael

betwo commented 4 years ago

Hi @michaelchi08,

this is definitely possible, we have had the same use case a few years ago.

Since you are planning to navigate into an unknown environment, you will need a robust obstacle detection system. This needs to be published as a point cloud of obstacle points, by default on the <robot-namespace>/obstacle_cloud topic.

GeRoNa uses this obstacle information in addition to the map that Hector SLAM will generate during path planning.

Useful places to start looking might be:

Basic laser scan to point cloud node: https://github.com/cogsys-tuebingen/gerona/tree/6d60f11860b39ca283c491710b0b77a2f353e3c4/tools/scan2cloud

Launch of the basic obstacle detector: https://github.com/cogsys-tuebingen/gerona/blob/6d60f11860b39ca283c491710b0b77a2f353e3c4/navigation_launch/launch/navigation.launch

3D obstacle detection (not sure of this applies to your use case) https://github.com/cogsys-tuebingen/csapex_generic_obstacle_detection

I hope this helps you getting started. Please do not hesitate to ask any follow-up questions :)

michaelchi08 commented 4 years ago

hectorslam gerona frames.pdf

Thanks for your help and suggestion. I'm getting this error. Information about my platform: -> 2d lidar: sick 551 -> skid steer drive

error i'm seeing:

mchikamc@mchikamc:~/catkin_ws/src/gerona/navigation_launch/launch$ export ROBOT_CONTROLLER=differential mchikamc@mchikamc:~/catkin_ws/src/gerona/navigation_launch/launch$ roslaunch rviz_controlled.launch ... logging to /home/mchikamc/.ros/log/167c79a8-4a24-11ea-afcb-48f8b368dfd3/roslaunch-mchikamc-4739.log Checking log directory for disk usage. This may take awhile. Press Ctrl-C to interrupt Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://mchikamc:36587/

SUMMARY

CLEAR PARAMETERS

PARAMETERS

NODES / highlevel_dummy (path_control/highlevel_dummy) path_control_node (path_control/path_control_node) path_follower (path_follower/path_follower_node) path_planner (path_planner/path_planner_node)

ROS_MASTER_URI=http://localhost:11311

process[highlevel_dummy-1]: started with pid [4756] process[path_planner-2]: started with pid [4757] process[path_control_node-3]: started with pid [4758] process[path_follower-4]: started with pid [4765] [ INFO] [1581134741.032981356]: Initialisation done. [ INFO] [1581134741.066595355]: using map topic map [ INFO] [1581134741.071865216]: subscribing to obstacle cloud topic /obstacle_cloud [ INFO] [1581134741.103274874]: oversearch distance is 0.05 [ INFO] [1581134741.105909360]: planner uses algorithm: 2d [ INFO] [1581134741.139347434]: Use Supervisor 'PathLookout' [ INFO] [1581134741.140080654]: Use Supervisor 'DistanceToPath' [ INFO] [1581134741.140119643]: Initialisation done. [ INFO] [1581134741.324115076]: listening for goal @ /move_base_simple/goal [ INFO] [1581134741.325163020]: failure mode is REPLAN [ INFO] [1581134741.325200416]: Client is set up [ INFO] [1581134760.803708735]: Send goal... [ INFO] [1581134760.808516632]: goal: planning_algorithm: data: planning_channel: data: type: 0 pose: header: seq: 0 stamp: 1581134760.803441619 frame_id: map pose: position: x: 3.16284 y: -0.163906 z: 0 orientation: x: 0 y: 0 z: -0.0263136 w: 0.999654 map: header: seq: 0 stamp: 0.000000000 frame_id: info: map_load_time: 0.000000000 resolution: 0 width: 0 height: 0 origin: position: x: 0 y: 0 z: 0 orientation: x: 0 y: 0 z: 0 w: 0 data[] map_search_min_value: 0 map_search_min_candidates: 0 min_dist: 0

[ INFO] [1581134760.917265054]: Start Action! Requested velocity: 1 [ INFO] [1581134760.917421366]: Goal just went active [ INFO] [1581134761.017425448]: Wait for follow_path action server... [ INFO] [1581134761.017514567]: got request with channel [ INFO] [1581134761.017541735]: remapped channel to /plan_path [ INFO] [1581134761.024788947]: waiting for planner @ /plan_path [ INFO] [1581134761.340274064]: waiting for path [ INFO] [1581134761.654580892]: still planning [ INFO] [1581134761.840403800]: still waiting for path [ INFO] [1581134761.854962479]: path planning took 481ms [ERROR] [1581134761.855166354]: Path planner failed. Final state: ABORTED [ WARN] [1581134761.955321911]: No path found. Abort goal. [ INFO] [1581134761.955528585]: DONE with action state ABORTED [ WARN] [1581134761.955573182]: Did not reach goal :( [ INFO] [1581134761.955610482]: Result code: 6 NO_PATH_FOUND [ INFO] [1581134761.955637484]: Additional Text: [ INFO] [1581134795.687412976]: Send goal... [ INFO] [1581134795.690703720]: goal: planning_algorithm: data: planning_channel: data: type: 0 pose: header: seq: 1 stamp: 1581134795.687234139 frame_id: map pose: position: x: 3.63973 y: -0.0157652 z: 0 orientation: x: 0 y: 0 z: -0.0116141 w: 0.999933 map: header: seq: 0 stamp: 0.000000000 frame_id: info: map_load_time: 0.000000000 resolution: 0 width: 0 height: 0 origin: position: x: 0 y: 0 z: 0 orientation: x: 0 y: 0 z: 0 w: 0 data[] map_search_min_value: 0 map_search_min_candidates: 0 min_dist: 0

[ INFO] [1581134795.800516896]: Start Action! Requested velocity: 1 [ INFO] [1581134795.800607813]: Goal just went active [ INFO] [1581134795.900645862]: Wait for follow_path action server... [ INFO] [1581134795.900720953]: got request with channel [ INFO] [1581134795.900739792]: remapped channel to /plan_path [ INFO] [1581134795.900756679]: waiting for planner @ /plan_path [ INFO] [1581134796.000937073]: waiting for path [ INFO] [1581134796.194020526]: still planning [ INFO] [1581134796.494230685]: path planning took 488ms [ERROR] [1581134796.494319382]: Path planner failed. Final state: ABORTED [ WARN] [1581134796.594445325]: No path found. Abort goal. [ INFO] [1581134796.594629798]: DONE with action state ABORTED [ WARN] [1581134796.594659971]: Did not reach goal :( [ INFO] [1581134796.594675035]: Result code: 6 NO_PATH_FOUND [ INFO] [1581134796.594696531]: Additional Text:

betwo commented 4 years ago

The kinematic path planner did not find a path. Looks like you had an obstacle in front of the robot: image

You already had the parameter /path_planner/use_unknown_cells: True, so the unknown map cells should not be the cause of failure.

The path planners used the following dimensions for collision checking:

/path_planner/size/backward: -0.6
/path_planner/size/forward: 0.2
/path_planner/size/width: 0.7

Do these match your robot's footprint? (Length 80cm, width 70cm) Roughly estimated from the screenshot, the gap is 50cm to 80cm so that should be a really close corner case. It could also be that your goal was to close to the wall, can't really say from the screenshot.

I would suggest to

  1. display the planning map and the visualization markers in RViz (shows the target footprint and also more information about the found path, including footprints along it)
  2. verify in your simulator (or in a free space) that the footprint matches your robot as closely as possible
michaelchi08 commented 4 years ago

Hey @betwo,

I changed the planner parameters to match my robot footprint. The footprint of my robot is 0.42m(Length) and 0.30 (width) but i'm still getting the error.

[ INFO] [1581273223.413644165]: Start Action! Requested velocity: 1 [ INFO] [1581273223.413738832]: Goal just went active [ INFO] [1581273223.513805473]: Wait for follow_path action server... [ INFO] [1581273223.513894763]: got request with channel [ INFO] [1581273223.513921557]: remapped channel to /plan_path [ INFO] [1581273223.520229476]: waiting for planner @ /plan_path [ INFO] [1581273223.825892127]: waiting for path [ INFO] [1581273224.167324054]: still planning [ INFO] [1581273224.325978772]: still waiting for path [ INFO] [1581273224.367683427]: path planning took 535ms [ERROR] [1581273224.367763958]: Path planner failed. Final state: ABORTED [ WARN] [1581273224.467923312]: No path found. Abort goal. [ INFO] [1581273224.468210281]: DONE with action state ABORTED [ WARN] [1581273224.468254868]: Did not reach goal :( [ INFO] [1581273224.468288109]: Result code: 6 NO_PATH_FOUND [ INFO] [1581273224.468307384]: Additional Text:

--------------ROSLAUNCH FOR PLANNER <?xml version="1.0"?>

The planning map is like this:

image

The hector map is like this: Screenshot from 2020-02-09 13-44-29

The wall in front the robot is ~ 2m. It should be able to plan this i think. Are there some parameters to tweak in the path planner?

I also did that export ROBOT_CONTROLLER = differential_orthexp before roslaunch rviz_controlled.launch

Thanks

michaelchi08 commented 4 years ago

Looks like it's working now :) I will test and play with it more over the next few days.

[ INFO] [1581277469.401787951]: Feedback: Moving [ WARN] [1581277470.401135866]: PathLookout has not received a valid obstacle cloud. No obstacle lookout is done. [ WARN] [1581277470.401256952]: no obstacle cloud is available [ INFO] [1581277470.421594046]: Feedback: Moving [ WARN] [1581277471.421160421]: PathLookout has not received a valid obstacle cloud. No obstacle lookout is done. [ WARN] [1581277471.421311914]: no obstacle cloud is available [ INFO] [1581277471.421787574]: Feedback: Moving [ WARN] [1581277472.441168714]: PathLookout has not received a valid obstacle cloud. No obstacle lookout is done. [ WARN] [1581277472.441356078]: no obstacle cloud is available [ INFO] [1581277472.441699953]: Feedback: Moving [ WARN] [1581277473.461132288]: PathLookout has not received a valid obstacle cloud. No obstacle lookout is done. [ WARN] [1581277473.461253175]: no obstacle cloud is available [ INFO] [1581277473.461628366]: Feedback: Moving [ WARN] [1581277474.481124446]: PathLookout has not received a valid obstacle cloud. No obstacle lookout is done. [ WARN] [1581277474.481233788]: no obstacle cloud is available [ INFO] [1581277474.481526972]: Feedback: Moving [ WARN] [1581277475.481132030]: PathLookout has not received a valid obstacle cloud. No obstacle lookout is done. [ WARN] [1581277475.481259672]: no obstacle cloud is available [ INFO] [1581277475.481609177]: Feedback: Moving [ WARN] [1581277476.501173036]: PathLookout has not received a valid obstacle cloud. No obstacle lookout is done. [ WARN] [1581277476.501417147]: no obstacle cloud is available [ INFO] [1581277476.501875567]: Feedback: Moving [ WARN] [1581277477.521156265]: PathLookout has not received a valid obstacle cloud. No obstacle lookout is done. [ WARN] [1581277477.521284963]: no obstacle cloud is available [ INFO] [1581277477.521509810]: Feedback: Moving [ WARN] [1581277478.521150197]: PathLookout has not received a valid obstacle cloud. No obstacle lookout is done. [ WARN] [1581277478.541137870]: no obstacle cloud is available [ INFO] [1581277478.541487536]: Feedback: Moving [ WARN] [1581277479.521189959]: PathLookout has not received a valid obstacle cloud. No obstacle lookout is done. [ WARN] [1581277479.541250660]: no obstacle cloud is available [ INFO] [1581277479.541813342]: Feedback: Moving [ WARN] [1581277480.541133140]: PathLookout has not received a valid obstacle cloud. No obstacle lookout is done. [ WARN] [1581277480.541262068]: no obstacle cloud is available [ INFO] [1581277480.561664123]: Feedback: Moving [ WARN] [1581277481.541140973]: PathLookout has not received a valid obstacle cloud. No obstacle lookout is done. [ WARN] [1581277481.541282502]: no obstacle cloud is available [ INFO] [1581277481.581565661]: Feedback: Moving [ WARN] [1581277482.561171044]: PathLookout has not received a valid obstacle cloud. No obstacle lookout is done. [ WARN] [1581277482.561342135]: no obstacle cloud is available [ INFO] [1581277482.581604326]: Feedback: Moving [ WARN] [1581277483.581139752]: PathLookout has not received a valid obstacle cloud. No obstacle lookout is done. [ WARN] [1581277483.581253395]: no obstacle cloud is available [ INFO] [1581277483.601335808]: Feedback: Moving [ WARN] [1581277484.581151987]: PathLookout has not received a valid obstacle cloud. No obstacle lookout is done. [ WARN] [1581277484.581332567]: no obstacle cloud is available [ INFO] [1581277484.601494300]: Feedback: Moving [ WARN] [1581277485.601128587]: PathLookout has not received a valid obstacle cloud. No obstacle lookout is done. [ WARN] [1581277485.601272220]: no obstacle cloud is available [ INFO] [1581277485.601545343]: Feedback: Moving [ WARN] [1581277486.621128589]: PathLookout has not received a valid obstacle cloud. No obstacle lookout is done. [ WARN] [1581277486.621242709]: no obstacle cloud is available [ INFO] [1581277486.621651801]: Feedback: Moving [ WARN] [1581277487.621166896]: PathLookout has not received a valid obstacle cloud. No obstacle lookout is done. [ WARN] [1581277487.621312736]: no obstacle cloud is available [ INFO] [1581277487.621749588]: Feedback: Moving [ WARN] [1581277488.641139571]: PathLookout has not received a valid obstacle cloud. No obstacle lookout is done. [ WARN] [1581277488.641282947]: no obstacle cloud is available [ INFO] [1581277488.641537660]: Feedback: Moving [ WARN] [1581277489.641153575]: PathLookout has not received a valid obstacle cloud. No obstacle lookout is done. [ INFO] [1581277489.641774552]: Feedback: Moving [ WARN] [1581277489.661199266]: no obstacle cloud is available [ WARN] [1581277490.661126929]: PathLookout has not received a valid obstacle cloud. No obstacle lookout is done. [ WARN] [1581277490.661242550]: no obstacle cloud is available [ INFO] [1581277490.661595453]: Feedback: Moving [ WARN] [1581277491.681128755]: PathLookout has not received a valid obstacle cloud. No obstacle lookout is done. [ WARN] [1581277491.681275908]: no obstacle cloud is available [ INFO] [1581277491.681637600]: Feedback: Moving [ WARN] [1581277492.701170135]: PathLookout has not received a valid obstacle cloud. No obstacle lookout is done. [ WARN] [1581277492.701329566]: no obstacle cloud is available [ INFO] [1581277492.701877085]: Feedback: Moving [ WARN] [1581277493.721192722]: PathLookout has not received a valid obstacle cloud. No obstacle lookout is done. [ WARN] [1581277493.721377620]: no obstacle cloud is available [ INFO] [1581277493.721868582]: Feedback: Moving [ WARN] [1581277494.741150307]: PathLookout has not received a valid obstacle cloud. No obstacle lookout is done. [ WARN] [1581277494.741273228]: no obstacle cloud is available [ INFO] [1581277494.741585552]: Feedback: Moving [ WARN] [1581277495.741186499]: PathLookout has not received a valid obstacle cloud. No obstacle lookout is done. [ WARN] [1581277495.741404324]: no obstacle cloud is available [ INFO] [1581277495.741811486]: Feedback: Moving [ WARN] [1581277496.761171038]: PathLookout has not received a valid obstacle cloud. No obstacle lookout is done. [ WARN] [1581277496.761300770]: no obstacle cloud is available [ INFO] [1581277496.761860681]: Feedback: Moving [ WARN] [1581277497.781143170]: PathLookout has not received a valid obstacle cloud. No obstacle lookout is done. [ WARN] [1581277497.781280576]: no obstacle cloud is available [ INFO] [1581277497.781774280]: Feedback: Moving [ WARN] [1581277498.801128141]: PathLookout has not received a valid obstacle cloud. No obstacle lookout is done. [ WARN] [1581277498.801281674]: no obstacle cloud is available [ INFO] [1581277498.801557673]: Feedback: Moving [ WARN] [1581277499.801132986]: PathLookout has not received a valid obstacle cloud. No obstacle lookout is done. [ WARN] [1581277499.821194157]: no obstacle cloud is available [ INFO] [1581277499.821681210]: Feedback: Moving [ WARN] [1581277500.801135081]: PathLookout has not received a valid obstacle cloud. No obstacle lookout is done. [ WARN] [1581277500.841178935]: no obstacle cloud is available [ INFO] [1581277500.841620336]: Feedback: Moving [ WARN] [1581277501.821125927]: PathLookout has not received a valid obstacle cloud. No obstacle lookout is done. [ WARN] [1581277501.841190555]: no obstacle cloud is available [ INFO] [1581277501.861441698]: Feedback: Moving [ WARN] [1581277502.821166912]: PathLookout has not received a valid obstacle cloud. No obstacle lookout is done. [ WARN] [1581277502.841222572]: no obstacle cloud is available [ INFO] [1581277502.861606315]: Feedback: Moving [ WARN] [1581277503.841145408]: PathLookout has not received a valid obstacle cloud. No obstacle lookout is done. [ WARN] [1581277503.841292359]: no obstacle cloud is available [ INFO] [1581277503.861679796]: Feedback: Moving [ WARN] [1581277504.841165396]: PathLookout has not received a valid obstacle cloud. No obstacle lookout is done. [ WARN] [1581277504.841317912]: no obstacle cloud is available

betwo commented 4 years ago

Thanks for the feedback that it's working now. Did you have to change anything to make it work?

Looks like you don't detect any obstacles currently? You might want to activate the "scan2cloud" node for that, if the hector map is not generated quickly enough for obstacle avoidance.

michaelchi08 commented 4 years ago

i did setup the scan2cloud node but getting these erorrs which might be tf related:

[ INFO] [1581481992.979302793]: Client is set up [ERROR] [1581481993.129657753]: cannot transform front from frame laser to base_link [ INFO] [1581481993.425179396]: lookupTransform base_link to laser timed out. Could not transform laser scan into base_frame. [ERROR] [1581481993.495763158]: error transforming the obstacle cloud from base_link to map: the transformation between map and base_link does not exist. [ WARN] [1581481993.697156566]: cannot lookup relative transform from map to base_link at time 0.000000000. Using latest transform [ WARN] [1581482013.028621870]: cannot lookup relative transform from map to base_link at time 0.000000000. Using latest transform [ WARN] [1581482013.129365279]: cannot lookup relative transform from map to base_link at time 0.000000000. Using latest transform [ WARN] [1581482013.229847183]: cannot lookup relative transform from map to base_link at time 0.000000000. Using latest transform [ WARN] [1581482013.330143235]: cannot lookup relative transform from map to base_link at time 0.000000000. Using latest transform [ WARN] [1581482013.513095714]: cannot lookup relative transform from map to base_link at time 0.000000000. Using latest transform [ WARN] [1581482013.614084252]: cannot lookup relative transform from map to base_link at time 0.000000000. Using latest transform [ WARN] [1581482048.399195093]: cannot lookup relative transform from map to base_link at time 0.000000000. Using latest transform [ WARN] [1581482061.560796961]: cannot lookup relative transform from map to base_link at time 0.000000000. Using latest transform [ WARN] [1581482062.154080526]: cannot lookup relative transform from map to base_link at time 0.000000000. Using latest transform

This is my launch. Can you explain what these topics do /scan/front/filtered and cloud/total?

<node name="obstacle_cloud" type="scan2cloud_node" pkg="scan2cloud"
      output="screen" if="$(arg use_laser_obstacles)">
    **<remap from="/scan/front/filtered" to="/scan" />
    <remap from="cloud/total" to="obstacle_cloud" />**
</node>
betwo commented 4 years ago

scan2cloud_node:

The scan2cloud_node takes multiple sensor_msgs::LaserScan messages as input and generates a single sensor_msgs::PointCloud2 as output. Currently, the code is written for a two sensor setup, but the concept can be scaled arbitrarily. All input scans are first projected into 3d points relative to the robot's base_link, then merged into a single point cloud in the same frame.

We assume that the laser scans are already filtered, i.e. the footprint of the robot is not included in the scan and thus all measurements are obstacle points. See the scan filters package and e.g. this example launch file.

The topics used are the following:

Inputs:

Output:

Needed transforms are base_link -> scan_front and base_link -> scan_back where scan_front is the frst scan's coordinate frame and scan_back is the second scan's frame.

With those transforms in place, you should be able to visualize the point cloud in RViz.

Remapping in your example

    <remap from="/scan/front/filtered" to="/scan" />
    <remap from="cloud/total" to="obstacle_cloud" />

This just remaps the I/O topics of the node. With this in place, the node reads /scan (unfiltered) and outputs /obstacle_cloud.

The error message 'error transforming ...'

However, the error message error transforming the obstacle cloud from base_link to map is not generated by the scan2cloud node, but by the path follower node. Looks like one link in your tf tree is missing.

Could you please provide a snapshot of your frames? (rosrun tf view_frames)

michaelchi08 commented 4 years ago

Thank you for help man. Really grateful and appreciated.

I will send you the tf frames later. I'm using the 2D sick scanner with /scan topic. In my case i only have a front facing laser scanner. So i should just add base_link -> scan_front to my tf and these errors should go away. Essentially, my /scan/front/filtered" is the "/scan" topic!?

if i had a 3d sensor, it would be the same concept? I know you referred a project previously.

betwo commented 4 years ago

So i should just add base_link -> scan_front to my tf and these errors should go away.

Right. The tree should generally allow you to transform every data source into your inertial frame(s), i.e. /odom and /map.

Essentially, my /scan/front/filtered" is the "/scan" topic!?

Yes, you can use /scan directly, if your scanner does not measure parts of your robot and is not tilted towards the floor.

if i had a 3d sensor, it would be the same concept? I know you referred a project previously.

Exactly. That is why we chose the generic point cloud interface. With a 3d scanner, you might need a more specialized algorithm, because you most likely also measure the floor, which must not be part of the obstacle cloud, otherwise the robot will not move a bit :wink: (The referred project was done with a 3d time-of-flight flash lidar)