Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Help using the package with Hector SLAM #22

Closed
michaelchi08 opened this issue Feb 7, 2020 · 10 comments
Closed

Help using the package with Hector SLAM #22

michaelchi08 opened this issue Feb 7, 2020 · 10 comments

Comments

@michaelchi08
Copy link

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 betwo self-assigned this Feb 7, 2020
@betwo
Copy link
Member

betwo commented Feb 7, 2020

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
Copy link
Author

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

  • /path_control_node/

PARAMETERS

  • /highlevel_dummy/failure_mode: replan
  • /highlevel_dummy/target_speed: 1.0
  • /path_control_node/nonaction_velocity: 1.0
  • /path_control_node/num_replan_attempts: 10
  • /path_follower/collision_avoider/collision_box/crit_length: 0.6
  • /path_follower/collision_avoider/collision_box/max_length: 1.5
  • /path_follower/collision_avoider/collision_box/min_length: 1.0
  • /path_follower/collision_avoider/collision_box/velocity_factor: 1.0
  • /path_follower/collision_avoider/collision_box/velocity_saturation: 1.0
  • /path_follower/collision_avoider/collision_box/width: 0.4
  • /path_follower/controller/2steer_inputscaling/k_backward: 7.0
  • /path_follower/controller/2steer_inputscaling/k_curv: 0.0
  • /path_follower/controller/2steer_inputscaling/k_forward: 4.5
  • /path_follower/controller/2steer_inputscaling/k_g: 0.5
  • /path_follower/controller/2steer_inputscaling/k_o: 0.0
  • /path_follower/controller/2steer_inputscaling/k_w: 0.0
  • /path_follower/controller/2steer_inputscaling/look_ahead_dist: 1.5
  • /path_follower/controller/2steer_inputscaling/max_steering_angle: 0.52359877559
  • /path_follower/controller/2steer_inputscaling/max_steering_angle_speed: 4.0
  • /path_follower/controller/2steer_inputscaling/obst_threshold: 3.0
  • /path_follower/controller/2steer_inputscaling/vehicle_length: 0.34
  • /path_follower/controller/2steer_purepursuit/k_backward: 0.2
  • /path_follower/controller/2steer_purepursuit/k_curv: 0.0
  • /path_follower/controller/2steer_purepursuit/k_forward: 0.2
  • /path_follower/controller/2steer_purepursuit/k_g: 0.5
  • /path_follower/controller/2steer_purepursuit/k_o: 0.0
  • /path_follower/controller/2steer_purepursuit/k_w: 0.0
  • /path_follower/controller/2steer_purepursuit/look_ahead_dist: 1.5
  • /path_follower/controller/2steer_purepursuit/obst_threshold: 3.0
  • /path_follower/controller/2steer_purepursuit/vehicle_length: 0.34
  • /path_follower/controller/2steer_stanley/k_backward: 1.6
  • /path_follower/controller/2steer_stanley/k_curv: 0.0
  • /path_follower/controller/2steer_stanley/k_forward: 1.6
  • /path_follower/controller/2steer_stanley/k_g: 0.5
  • /path_follower/controller/2steer_stanley/k_o: 0.0
  • /path_follower/controller/2steer_stanley/k_w: 0.0
  • /path_follower/controller/2steer_stanley/look_ahead_dist: 1.5
  • /path_follower/controller/2steer_stanley/max_steering_angle: 0.52359877559
  • /path_follower/controller/2steer_stanley/obst_threshold: 3.0
  • /path_follower/controller/2steer_stanley/vehicle_length: 0.34
  • /path_follower/controller/PBR/k1: 5.0
  • /path_follower/controller/PBR/k2: 5.0
  • /path_follower/controller/PBR/k_curv: 0.0
  • /path_follower/controller/PBR/k_g: 0.0
  • /path_follower/controller/PBR/k_o: 0
  • /path_follower/controller/PBR/k_w: 0.0
  • /path_follower/controller/PBR/look_ahead_dist: 1.5
  • /path_follower/controller/PBR/max_angular_velocity: 0.6
  • /path_follower/controller/PBR/obst_threshold: 3.0
  • /path_follower/controller/ackermann_inputscaling/factor_k1: 1.0
  • /path_follower/controller/ackermann_inputscaling/factor_k2: 3.0
  • /path_follower/controller/ackermann_inputscaling/factor_k3: 3.0
  • /path_follower/controller/ackermann_inputscaling/factor_steering_angle: 1.0
  • /path_follower/controller/ackermann_inputscaling/k_backward: 10.0
  • /path_follower/controller/ackermann_inputscaling/k_curv: 0.0
  • /path_follower/controller/ackermann_inputscaling/k_forward: 4.5
  • /path_follower/controller/ackermann_inputscaling/k_g: 0.5
  • /path_follower/controller/ackermann_inputscaling/k_o: 0.0
  • /path_follower/controller/ackermann_inputscaling/k_w: 0.0
  • /path_follower/controller/ackermann_inputscaling/look_ahead_dist: 1.5
  • /path_follower/controller/ackermann_inputscaling/max_steering_angle: 0.52359877559
  • /path_follower/controller/ackermann_inputscaling/max_steering_angle_speed: 4.0
  • /path_follower/controller/ackermann_inputscaling/obst_threshold: 3.0
  • /path_follower/controller/ackermann_inputscaling/vehicle_length: 0.34
  • /path_follower/controller/ackermann_orthexp/k: 0.2
  • /path_follower/controller/ackermann_orthexp/k_curv: 0.0
  • /path_follower/controller/ackermann_orthexp/k_g: 0.01
  • /path_follower/controller/ackermann_orthexp/k_o: 0.0
  • /path_follower/controller/ackermann_orthexp/k_w: 0.0
  • /path_follower/controller/ackermann_orthexp/look_ahead_dist: 1.0
  • /path_follower/controller/ackermann_orthexp/obst_threshold: 3.0
  • /path_follower/controller/ackermann_pid/pid/ki: 0.001
  • /path_follower/controller/ackermann_pid/pid/kp: 1.5
  • /path_follower/controller/ackermann_purepursuit/factor_lookahead_distance_backward: 0.3
  • /path_follower/controller/ackermann_purepursuit/factor_lookahead_distance_forward: 0.3
  • /path_follower/controller/ackermann_purepursuit/factor_steering_angle: 1.0
  • /path_follower/controller/ackermann_purepursuit/k_curv: 0.0
  • /path_follower/controller/ackermann_purepursuit/k_g: 0.1
  • /path_follower/controller/ackermann_purepursuit/k_o: 0.0
  • /path_follower/controller/ackermann_purepursuit/k_w: 0.0
  • /path_follower/controller/ackermann_purepursuit/look_ahead_dist: 1.0
  • /path_follower/controller/ackermann_purepursuit/obst_threshold: 3.0
  • /path_follower/controller/ackermann_purepursuit/vehicle_length: 0.34
  • /path_follower/controller/ackermann_stanley/factor_steering_angle: 1.0
  • /path_follower/controller/ackermann_stanley/k_backward: 3.0
  • /path_follower/controller/ackermann_stanley/k_curv: 0.0
  • /path_follower/controller/ackermann_stanley/k_forward: 1.0
  • /path_follower/controller/ackermann_stanley/k_g: 0.5
  • /path_follower/controller/ackermann_stanley/k_o: 0.0
  • /path_follower/controller/ackermann_stanley/k_w: 0.0
  • /path_follower/controller/ackermann_stanley/look_ahead_dist: 1.0
  • /path_follower/controller/ackermann_stanley/obst_threshold: 3.0
  • /path_follower/controller/ackermann_stanley/vehicle_length: 0.34
  • /path_follower/controller/differential_orthexp/k_curv: 0.0
  • /path_follower/controller/differential_orthexp/k_g: 0.5
  • /path_follower/controller/differential_orthexp/k_o: 0.0
  • /path_follower/controller/differential_orthexp/k_w: 0.0
  • /path_follower/controller/differential_orthexp/look_ahead_dist: 1.0
  • /path_follower/controller/differential_orthexp/obst_threshold: 3.0
  • /path_follower/controller/dynamic_window/T_dwa: 0.26
  • /path_follower/controller/dynamic_window/ang_acc: 2.0
  • /path_follower/controller/dynamic_window/ang_dec: 0.1
  • /path_follower/controller/dynamic_window/angle_fact: 0.1
  • /path_follower/controller/dynamic_window/disobst_fact: 10.0
  • /path_follower/controller/dynamic_window/fact_T: 10.0
  • /path_follower/controller/dynamic_window/initial_vel_fact: 1e-3
  • /path_follower/controller/dynamic_window/lin_acc: 2.0
  • /path_follower/controller/dynamic_window/lin_dec: 0.5
  • /path_follower/controller/dynamic_window/max_ang_vel: 0.5
  • /path_follower/controller/dynamic_window/obst_dist_thresh: 0.8
  • /path_follower/controller/dynamic_window/step_T: 0.13
  • /path_follower/controller/dynamic_window/v_fact: 0.1
  • /path_follower/controller/dynamic_window/v_step: 0.1
  • /path_follower/controller/dynamic_window/w_step: 0.1
  • /path_follower/controller/kinematic_hbz/alpha_l: 0.9
  • /path_follower/controller/kinematic_hbz/alpha_r: 0.9128
  • /path_follower/controller/kinematic_hbz/b: 0.2
  • /path_follower/controller/kinematic_hbz/epsilon: 0.5
  • /path_follower/controller/kinematic_hbz/k1: 8.0
  • /path_follower/controller/kinematic_hbz/k2: 40
  • /path_follower/controller/kinematic_hbz/k_curv: 0.0
  • /path_follower/controller/kinematic_hbz/k_g: 0.05
  • /path_follower/controller/kinematic_hbz/k_o: 0.0
  • /path_follower/controller/kinematic_hbz/k_w: 0.0
  • /path_follower/controller/kinematic_hbz/lambda: 1
  • /path_follower/controller/kinematic_hbz/look_ahead_dist: 1.5
  • /path_follower/controller/kinematic_hbz/max_angular_velocity: 0.5
  • /path_follower/controller/kinematic_hbz/obst_threshold: 3.0
  • /path_follower/controller/kinematic_hbz/theta_a: 2.0*0.78539816339
  • /path_follower/controller/kinematic_hbz/x_ICR: 0.2809
  • /path_follower/controller/kinematic_hbz/y_ICR_l: 0.38178
  • /path_follower/controller/kinematic_hbz/y_ICR_r: -0.487
  • /path_follower/controller/kinematic_hbz_tt/alpha_l: 0.9
  • /path_follower/controller/kinematic_hbz_tt/alpha_r: 0.9128
  • /path_follower/controller/kinematic_hbz_tt/b: 0.2
  • /path_follower/controller/kinematic_hbz_tt/des_dist: 2.0
  • /path_follower/controller/kinematic_hbz_tt/epsilon: 0.5
  • /path_follower/controller/kinematic_hbz_tt/k1: 0.5
  • /path_follower/controller/kinematic_hbz_tt/k2: 30
  • /path_follower/controller/kinematic_hbz_tt/k_curv: 0.0
  • /path_follower/controller/kinematic_hbz_tt/k_g: 0.0
  • /path_follower/controller/kinematic_hbz_tt/k_l: 0.2
  • /path_follower/controller/kinematic_hbz_tt/k_o: 0.0
  • /path_follower/controller/kinematic_hbz_tt/k_w: 0.0
  • /path_follower/controller/kinematic_hbz_tt/lambda: 1
  • /path_follower/controller/kinematic_hbz_tt/look_ahead_dist: 1.5
  • /path_follower/controller/kinematic_hbz_tt/max_angular_velocity: 0.5
  • /path_follower/controller/kinematic_hbz_tt/theta_a: 2.0*0.78539816339
  • /path_follower/controller/kinematic_hbz_tt/x_ICR: 0.2809
  • /path_follower/controller/kinematic_hbz_tt/y_ICR_l: 0.38178
  • /path_follower/controller/kinematic_hbz_tt/y_ICR_r: -0.487
  • /path_follower/controller/kinematic_slp/b: 0.2
  • /path_follower/controller/kinematic_slp/epsilon: 0.5
  • /path_follower/controller/kinematic_slp/gamma: 5.0
  • /path_follower/controller/kinematic_slp/k1: 3.0
  • /path_follower/controller/kinematic_slp/k2: 5.0
  • /path_follower/controller/kinematic_slp/k_curv: 0.0
  • /path_follower/controller/kinematic_slp/k_g: 0.5
  • /path_follower/controller/kinematic_slp/k_o: 0.0
  • /path_follower/controller/kinematic_slp/k_w: 0.0
  • /path_follower/controller/kinematic_slp/look_ahead_dist: 1.0
  • /path_follower/controller/kinematic_slp/max_angular_velocity: 0.5
  • /path_follower/controller/kinematic_slp/obst_threshold: 3.0
  • /path_follower/controller/kinematic_slp/theta_a: 2*0.78539816339
  • /path_follower/controller/ofc/goal_x: 2.0
  • /path_follower/controller/ofc/goal_y: 0.0
  • /path_follower/controller/ofc/kd_ang: 0.1
  • /path_follower/controller/ofc/kd_lin: 0.02
  • /path_follower/controller/ofc/ki_ang: 0.1
  • /path_follower/controller/ofc/ki_lin: 0.01
  • /path_follower/controller/ofc/kp_ang: 0.5
  • /path_follower/controller/ofc/kp_lin: 0.2
  • /path_follower/controller/ofc/max_angular_velocity: 0.5
  • /path_follower/controller/omnidrive_orthexp/k: 2.0
  • /path_follower/controller/omnidrive_orthexp/k_curv: 0.005
  • /path_follower/controller/omnidrive_orthexp/k_g: 0.4
  • /path_follower/controller/omnidrive_orthexp/k_o: 0.2
  • /path_follower/controller/omnidrive_orthexp/k_w: 0.75
  • /path_follower/controller/omnidrive_orthexp/kd: 1.0
  • /path_follower/controller/omnidrive_orthexp/kp: 1.5
  • /path_follower/controller/omnidrive_orthexp/look_ahead_dist: 2.0
  • /path_follower/controller/omnidrive_orthexp/obst_threshold: 3.0
  • /path_follower/controller/potential_field/dist_thresh: 2.5
  • /path_follower/controller/potential_field/kAtt: 1.0
  • /path_follower/controller/potential_field/kRep: 4.0
  • /path_follower/controller/potential_field/max_angular_velocity: 0.5
  • /path_follower/controller/potential_field_tt/kAtt: 0.2
  • /path_follower/controller/potential_field_tt/kRep: 0.5
  • /path_follower/controller/potential_field_tt/max_angular_velocity: 0.5
  • /path_follower/controller/unicycle_inputscaling/k: 2.0
  • /path_follower/controller/unicycle_inputscaling/max_angular_velocity: 0.8
  • /path_follower/controller/unicycle_inputscaling/vehicle_length: 0.5
  • /path_follower/controller_type: differential
  • /path_follower/goal_tolerance: 0.1
  • /path_follower/local_planner/algorithm: NULL
  • /path_follower/max_velocity: 2.5
  • /path_follower/min_velocity: 0.5
  • /path_follower/supervisor/distance_to_path/max_dist: 2.0
  • /path_follower/supervisor/path_lookout/obstacle_scale_distance: 1.0
  • /path_follower/supervisor/path_lookout/obstacle_scale_lifetime: 5.0
  • /path_follower/supervisor/path_lookout/path_width: 0.5
  • /path_follower/supervisor/use_distance_to_path: True
  • /path_follower/supervisor/use_path_lookout: True
  • /path_follower/supervisor/use_waypoint_timeout: False
  • /path_follower/waypoint_tolerance: 0.8
  • /path_planner/algorithm: 2d
  • /path_planner/grow_obstacles: 0.2
  • /path_planner/map_service: dynamic_map
  • /path_planner/map_topic: map
  • /path_planner/oversearch_distance: 0.05
  • /path_planner/penalty/backward: 1.0
  • /path_planner/penalty/turn: 0.0
  • /path_planner/postprocess: True
  • /path_planner/preprocess: True
  • /path_planner/render_open_cells: False
  • /path_planner/size/backward: -0.6
  • /path_planner/size/forward: 0.2
  • /path_planner/size/width: 0.7
  • /path_planner/use_cloud: True
  • /path_planner/use_collision_gridmap: False
  • /path_planner/use_cost_map: False
  • /path_planner/use_map_service: False
  • /path_planner/use_map_topic: True
  • /path_planner/use_scan_back: False
  • /path_planner/use_scan_front: True
  • /path_planner/use_unknown_cells: True
  • /rosdistro: melodic
  • /rosversion: 1.14.3

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
Copy link
Member

betwo commented Feb 9, 2020

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
Copy link
Author

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

    <param name="use_collision_gridmap" value="false" />
    <param name="grow_obstacles" value="0.2" />

    <param name="render_open_cells" value="false"/>

    <param name="penalty/backward" value="1.0" />
    <param name="penalty/turn" value="0.0" />

    <param name="oversearch_distance" value="0.05" />

    <param name="algorithm" value="2d" />

    <param name="preprocess" value="true" />
    <param name="postprocess" value="true" />

    <param name="use_cloud" value="true" />
    <param name="use_scan_front" value="true" />
    <param name="use_scan_back" value="false" />

    <param name="size/forward" value="0.21" />
    <param name="size/backward" value="-0.21" />
    <param name="size/width" value="0.30" />

    <remap from="scan/front/filtered" to="scan_filtered" />
</node>

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
Copy link
Author

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
Copy link
Member

betwo commented Feb 11, 2020

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
Copy link
Author

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
Copy link
Member

betwo commented Feb 12, 2020

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:

  • scan/front/filtered: Filtered laser scan forward facing, (non-obstacle points removed)
  • scan/back/filtered: Filtered laser scan rear facing, (non-obstacle points removed)

Output:

  • cloud/total: The merged point cloud in robot base frame

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
Copy link
Author

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
Copy link
Member

betwo commented Feb 13, 2020

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 😉
(The referred project was done with a 3d time-of-flight flash lidar)

@betwo betwo closed this as completed May 23, 2020
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
None yet
Development

No branches or pull requests

2 participants