search

leggedrobotics / art_planner

Local Navigation Planner for Legged Robots
BSD 3-Clause "New" or "Revised" License
125 stars 20 forks source link

Error: LazyPRMstar: There are no valid initial states! #4

Closed ArghyaChatterjee closed 2 years ago

ArghyaChatterjee commented 2 years ago

I am facing some problem during launching art planner. It can't start the planning. Though I am not expert, it looks like the merged pointlouds from 3 lidars on ANYmal_c are creating elevation map on top of it which is a problem. Is there a way to remove (or not included) those points on top of the robot from elevation map?? (Note: I have transformed and merged pointclouds from vlp-16, RSbpearl front and RSbpearl rear into single frame anymal_c.

arghya@arghya-Erazer-X7849-MD60379:~/ANYmal_ws$ roslaunch art_planner_ros art_planner.launch 
... logging to /home/arghya/.ros/log/b43862c4-c79a-11ec-923f-4485007e1d87/roslaunch-arghya-Erazer-X7849-MD60379-7913.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
started roslaunch server http://arghya-Erazer-X7849-MD60379:46873/

SUMMARY
========

PARAMETERS
 * /art_planner/objectives/clearance/cost_center: 5
 * /art_planner/objectives/clearance/cost_diag: 1
 * /art_planner/objectives/clearance/cost_lat: 2
 * /art_planner/objectives/clearance/cost_lon: 3
 * /art_planner/objectives/clearance/enable: False
 * /art_planner/objectives/clearance/weight: 10.0
 * /art_planner/objectives/custom_path_length/max_ang_vel: 0.5
 * /art_planner/objectives/custom_path_length/max_lat_vel: 0.1
 * /art_planner/objectives/custom_path_length/max_lon_vel: 0.5
 * /art_planner/objectives/custom_path_length/use_directional_cost: True
 * /art_planner/path_following/local_guidance_mode: walk
 * /art_planner/planner/elevation_layer: elevation
 * /art_planner/planner/lazy_prm_star_min_update/cleanup_when_not_planning: True
 * /art_planner/planner/lazy_prm_star_min_update/height_change_for_update: 0.05
 * /art_planner/planner/lazy_prm_star_min_update/invalidate_updated_graph_components: True
 * /art_planner/planner/n_threads: 4
 * /art_planner/planner/name: lazy_prm_star_min...
 * /art_planner/planner/plan_time: 0.01
 * /art_planner/planner/replan_freq: 1.0
 * /art_planner/planner/simplify_solution: True
 * /art_planner/planner/snap_goal_to_map: True
 * /art_planner/planner/start_goal_search/goal_radius: 0.5
 * /art_planner/planner/start_goal_search/n_iter: 1000
 * /art_planner/planner/start_goal_search/start_radius: 0.2
 * /art_planner/planner/traversability_layer: traversability
 * /art_planner/planner/traversability_thres: 0.5
 * /art_planner/planner/unknown_space_untraversable: True
 * /art_planner/robot/base_frame: anymal_c
 * /art_planner/robot/feet/offset/x: 0.362
 * /art_planner/robot/feet/offset/y: 0.225
 * /art_planner/robot/feet/offset/z: -0.525
 * /art_planner/robot/feet/reach/x: 0.4
 * /art_planner/robot/feet/reach/y: 0.3
 * /art_planner/robot/feet/reach/z: 0.15
 * /art_planner/robot/torso/height: 0.3
 * /art_planner/robot/torso/length: 1.07
 * /art_planner/robot/torso/offset/x: 0.0
 * /art_planner/robot/torso/offset/y: 0.0
 * /art_planner/robot/torso/offset/z: 0.04
 * /art_planner/robot/torso/width: 0.55
 * /art_planner/sampler/max_pitch_pert: 10
 * /art_planner/sampler/max_prob_unknown_samples: 0.1
 * /art_planner/sampler/max_roll_pert: 3.33
 * /art_planner/sampler/sample_from_distribution: True
 * /art_planner/sampler/use_inverse_vertex_density: True
 * /art_planner/sampler/use_max_prob_unknown_samples: True
 * /art_planner/verbose: True
 * /rosdistro: melodic
 * /rosversion: 1.14.13

NODES
  /
    art_planner (art_planner_ros/art_planner_ros_node)
    art_planner_plan_to_goal_client (art_planner_ros/plan_to_goal_client.py)

ROS_MASTER_URI=http://localhost:11311

process[art_planner-1]: started with pid [8180]
process[art_planner_plan_to_goal_client-2]: started with pid [8181]
Waiting for plan_to_goal server to appear...
Found server.

[ INFO] [1651226340.044097554, 419.200000000]: Received goal pose:
header: 
  seq: 2
  stamp: 418.604000000
  frame_id: odom
pose: 
  position: 
    x: 25.4173
    y: -4.3785
    z: 0
  orientation: 
    x: 0
    y: 0
    z: 0.0229904
    w: 0.999736

Grid map does not have "traversability" layer. Assuming traversable.
Invalidated 0 vertices
Invalidated 0/0 edges
Initial start state not valid. Trying to sample around it.

Epic fail, dude
Warning: LazyPRMstar: Skipping invalid start state (invalid state)
         at line 248 in /tmp/binarydeb/ros-melodic-ompl-1.4.2/src/ompl/base/src/Planner.cpp
Debug:   LazyPRMstar: Discarded start state Compound state [
RealVectorState [23.8531 -4.45529 -0.376203]
SO3State [-0.0396322 0.0409409 -0.0359117 0.997729]
]

Error:   LazyPRMstar: There are no valid initial states!
         at line 513 in /home/arghya/ANYmal_ws/src/art_planner/art_planner/src/planners/lazy_prm_star_min_update.cpp
Info:    No solution found after 0.000294 seconds

Screenshot from 2022-04-29 15-58-51

There is a warning on elevation mapping node:

arghya@arghya-Erazer-X7849-MD60379:~/ANYmal_ws$ roslaunch elevation_mapping_demos anymal_c_elevation_mapping.launch 
... logging to /home/arghya/.ros/log/b43862c4-c79a-11ec-923f-4485007e1d87/roslaunch-arghya-Erazer-X7849-MD60379-32616.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
started roslaunch server http://arghya-Erazer-X7849-MD60379:46457/

SUMMARY
========

PARAMETERS
 * /elevation_mapping/enable_visibility_cleanup: False
 * /elevation_mapping/fused_map_publishing_rate: 0.5
 * /elevation_mapping/init_submap_height_offset: 0.01
 * /elevation_mapping/initialization_method: 0
 * /elevation_mapping/initialize_elevation_map: False
 * /elevation_mapping/length_in_x: 6.0
 * /elevation_mapping/length_in_x_init_submap: 1.0
 * /elevation_mapping/length_in_y: 6.0
 * /elevation_mapping/length_in_y_init_submap: 1.0
 * /elevation_mapping/mahalanobis_distance_threshold: 2.5
 * /elevation_mapping/map_frame_id: odom
 * /elevation_mapping/margin_init_submap: 0.3
 * /elevation_mapping/max_variance: 0.05
 * /elevation_mapping/min_update_rate: 2.0
 * /elevation_mapping/min_variance: 0.0001
 * /elevation_mapping/multi_height_noise: 0.001
 * /elevation_mapping/point_cloud_topic: /anymal_c/combine...
 * /elevation_mapping/position_x: 0.0
 * /elevation_mapping/position_y: 0.0
 * /elevation_mapping/postprocessor_pipeline: [{'type': 'gridMa...
 * /elevation_mapping/resolution: 0.1
 * /elevation_mapping/robot_base_frame_id: anymal_c
 * /elevation_mapping/robot_motion_map_update/covariance_scale: 0.01
 * /elevation_mapping/robot_pose_cache_size: 200
 * /elevation_mapping/robot_pose_with_covariance_topic: /loam/pose
 * /elevation_mapping/scanning_duration: 1.0
 * /elevation_mapping/sensor_processor/beam_angle: 0.0006
 * /elevation_mapping/sensor_processor/beam_constant: 0.0015
 * /elevation_mapping/sensor_processor/ignore_points_above: 0.6
 * /elevation_mapping/sensor_processor/min_radius: 0.018
 * /elevation_mapping/sensor_processor/type: laser
 * /elevation_mapping/surface_normal_positive_axis: z
 * /elevation_mapping/target_frame_init_submap: anymal_c
 * /elevation_mapping/time_offset_for_point_cloud: 0.0
 * /elevation_mapping/time_tolerance: 1.0
 * /elevation_mapping/track_point_frame_id: anymal_c
 * /elevation_mapping/track_point_x: 0.0
 * /elevation_mapping/track_point_y: 0.0
 * /elevation_mapping/track_point_z: 0.0
 * /elevation_mapping/visibility_cleanup_rate: 1.0
 * /rosdistro: melodic
 * /rosversion: 1.14.13

NODES
  /
    elevation_mapping (elevation_mapping/elevation_mapping)

ROS_MASTER_URI=http://localhost:11311

process[elevation_mapping-1]: started with pid [344]
[ WARN] [1651227229.356009704]: Could not find the parameter: `algorithm`. Setting to default value: 'area'.
[ WARN] [1651227229.358008454]: Could not find the parameter: `parallelization_enabled`. Setting to default value: 'false'.
[ WARN] [1651227229.358576404]: Could not find the parameter: `thread_number`. Setting to default value: 'automatic'.
[ INFO] [1651227229.361199922]: Elevation mapping node started.
[ INFO] [1651227229.387342299]: Elevation map grid resized to 60 rows and 60 columns.
[ WARN] [1651227229.429014591]: Could not load the input sources configuration from parameter
 /elevation_mapping/input_sources, are you sure it was pushed to the parameter server? Assuming
 that you meant to leave it empty. Not subscribing to any inputs!

[ WARN] [1651227229.430182369]: Parameter 'point_cloud_topic' is deprecated, please use 'input_sources' instead.
[ INFO] [1651227229.449461756]: Elevation mapping node initializing ... 
[ WARN] [1651226932.425124466, 488.204000000]: Estimation radius is smaller than allowed by the map resolution (0.050000 < 0.050000)
lorenwel commented 2 years ago

Hi, the issue as you correctly stated is that the robot position is in collision due to the self-hits on the robot. The most proper way would be to use the robot_self_filter to filter all points which are self-hits. As a quick fix you can also try using a minimum distance filter on the point clouds which is less accurate but should also prevent self-hits.

Make sure that you still have the elevation map below the robot built after you filter all these points, otherwise you might be getting the same error due to holes in the elevation map.

ArghyaChatterjee commented 2 years ago

Hi, Thanks for your reply. I gave a passthrough filter and the problem seems to resolve. Here is how I did it (in case someone needs it in future:

    <node pkg="nodelet" type="nodelet" name="pcl_manager" args="manager" output="screen" respawn="true"/>
    <node pkg="nodelet" type="nodelet" name="pass_through" args="load pcl/PassThrough pcl_manager" output="screen" respawn="true">
        <remap from="~input" to="anymal_c/combined_transformed_point_cloud" />
        <remap from="~output" to="anymal_c/combined_transformed_point_cloud_filtered" />
        <rosparam subst_value="true">
            filter_field_name: z
            filter_limit_min: -0.55
            filter_limit_max: 0.3
            filter_limit_negative: True
            output_frame: anymal_c
        </rosparam>
    </node>

Screenshot from 2022-04-30 02-44-10

arghya@arghya-Erazer-X7849-MD60379:~/ANYmal_ws$ roslaunch art_planner_ros art_planner.launch 
WARNING: Package name "ODE" does not follow the naming conventions. It should start with a lower case letter and only contain lower case letters, digits, underscores, and dashes.
... logging to /home/arghya/.ros/log/d16b27dc-c7e5-11ec-9ea6-4485007e1d87/roslaunch-arghya-Erazer-X7849-MD60379-25067.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

WARNING: Package name "ODE" does not follow the naming conventions. It should start with a lower case letter and only contain lower case letters, digits, underscores, and dashes.
started roslaunch server http://arghya-Erazer-X7849-MD60379:45809/

SUMMARY
========

PARAMETERS
 * /art_planner/objectives/clearance/cost_center: 5
 * /art_planner/objectives/clearance/cost_diag: 1
 * /art_planner/objectives/clearance/cost_lat: 2
 * /art_planner/objectives/clearance/cost_lon: 3
 * /art_planner/objectives/clearance/enable: False
 * /art_planner/objectives/clearance/weight: 10.0
 * /art_planner/objectives/custom_path_length/max_ang_vel: 0.5
 * /art_planner/objectives/custom_path_length/max_lat_vel: 0.1
 * /art_planner/objectives/custom_path_length/max_lon_vel: 0.5
 * /art_planner/objectives/custom_path_length/use_directional_cost: True
 * /art_planner/path_following/local_guidance_mode: walk
 * /art_planner/planner/elevation_layer: elevation
 * /art_planner/planner/lazy_prm_star_min_update/cleanup_when_not_planning: True
 * /art_planner/planner/lazy_prm_star_min_update/height_change_for_update: 0.05
 * /art_planner/planner/lazy_prm_star_min_update/invalidate_updated_graph_components: True
 * /art_planner/planner/n_threads: 4
 * /art_planner/planner/name: lazy_prm_star_min...
 * /art_planner/planner/plan_time: 0.01
 * /art_planner/planner/replan_freq: 1.0
 * /art_planner/planner/simplify_solution: True
 * /art_planner/planner/snap_goal_to_map: True
 * /art_planner/planner/start_goal_search/goal_radius: 0.5
 * /art_planner/planner/start_goal_search/n_iter: 1000
 * /art_planner/planner/start_goal_search/start_radius: 0.2
 * /art_planner/planner/traversability_layer: traversability
 * /art_planner/planner/traversability_thres: 0.5
 * /art_planner/planner/unknown_space_untraversable: True
 * /art_planner/robot/base_frame: anymal_c
 * /art_planner/robot/feet/offset/x: 0.362
 * /art_planner/robot/feet/offset/y: 0.225
 * /art_planner/robot/feet/offset/z: -0.525
 * /art_planner/robot/feet/reach/x: 0.4
 * /art_planner/robot/feet/reach/y: 0.3
 * /art_planner/robot/feet/reach/z: 0.15
 * /art_planner/robot/torso/height: 0.3
 * /art_planner/robot/torso/length: 1.07
 * /art_planner/robot/torso/offset/x: 0.0
 * /art_planner/robot/torso/offset/y: 0.0
 * /art_planner/robot/torso/offset/z: 0.04
 * /art_planner/robot/torso/width: 0.55
 * /art_planner/sampler/max_pitch_pert: 10
 * /art_planner/sampler/max_prob_unknown_samples: 0.1
 * /art_planner/sampler/max_roll_pert: 3.33
 * /art_planner/sampler/sample_from_distribution: True
 * /art_planner/sampler/use_inverse_vertex_density: True
 * /art_planner/sampler/use_max_prob_unknown_samples: True
 * /art_planner/verbose: True
 * /rosdistro: melodic
 * /rosversion: 1.14.13

NODES
  /
    art_planner (art_planner_ros/art_planner_ros_node)
    art_planner_plan_to_goal_client (art_planner_ros/plan_to_goal_client.py)

ROS_MASTER_URI=http://localhost:11311

WARNING: Package name "ODE" does not follow the naming conventions. It should start with a lower case letter and only contain lower case letters, digits, underscores, and dashes.
process[art_planner-1]: started with pid [25290]
process[art_planner_plan_to_goal_client-2]: started with pid [25291]
Waiting for plan_to_goal server to appear...
Found server.
[ INFO] [1651261414.602862234, 1314.992000000]: Received goal pose:
header: 
  seq: 2
  stamp: 1314.992000000
  frame_id: odom
pose: 
  position: 
    x: 1.18169
    y: -0.375552
    z: 0
  orientation: 
    x: 0
    y: 0
    z: -0.264362
    w: 0.964424

Grid map does not have "traversability" layer. Assuming traversable.
Original goal [1.181689 -0.375552   0.000000], clipped to [1.181689 -0.375552   -0.376745].
Invalidated 0 vertices
Invalidated 0/0 edges
Initial start is valid
Debug:   LazyPRMstar: Planner range detected to be 3.708540
Info:    LazyPRMstar: Starting planning with 2 states already in datastructure
Info:    LazyPRMstar: Created 4 states
Info:    Solution found in 0.010320 seconds
Info:    SimpleSetup: Path simplification took 0.000005 seconds and changed from 2 to 2 states
Grid map does not have "traversability" layer. Assuming traversable.
Cleaned up for 9.41e+03ms
Added 573 new milestones.
Planned 677 paths between 573 pairs and removed 107 edges
Original goal [1.181689 -0.375552   0.000000], clipped to [1.181689 -0.375552   -0.377675].
Cleaned up for 22ms
Added 1 new milestones.
Planned 1 paths between 1 pairs and removed 0 edges
Invalidated 0 vertices
Invalidated 0/590 edges
Initial start is valid
Info:    LazyPRMstar: Starting planning with 580 states already in datastructure
Info:    LazyPRMstar: Created 1 states
Info:    Solution found in 0.015081 seconds
Info:    SimpleSetup: Path simplification took 0.041822 seconds and changed from 3 to 2 states

Now, for controlling ANYmal_c, I use cerberus_anymal_locomotion controller and control the robot using rosrun teleop_twist_keyboard teleop_twist_keyboard.py /cmd_vel:=/anymal_c/cmd_vel keyboard. In this case, what I understand is, from rviz or CLI, I publish a goal pose on /goal topic and art_planner subscribes to the /goal pose and publishes a art_planner_msgs/PlanToGoalActionGoal type message on /art_planner/plan_to_goal/goal topic. My question is how to convert this art_planner_msgs/PlanToGoalActionGoal type message on /art_planner/plan_to_goal/goal topic to motion controller readable geometery_msgs/Twist type message on /anymal_c/cmd_vel topic. Is there any opensource package available for that ?? Thanks in advance.


arghya@arghya-Erazer-X7849-MD60379:~/ANYmal_ws$ rostopic echo /art_planner/plan_to_goal/goal
WARNING: no messages received and simulated time is active.
Is /clock being published?
header: 
  seq: 2
  stamp: 
    secs: 1495
    nsecs:  80000000
  frame_id: ''
goal_id: 
  stamp: 
    secs: 1495
    nsecs:  80000000
  id: "/art_planner_plan_to_goal_client-2-1495.080"
goal: 
  goal: 
    header: 
      seq: 3
      stamp: 
        secs: 1495
        nsecs:  76000000
      frame_id: "odom"
    pose: 
      position: 
        x: 0.947381019592
        y: 2.03952217102
        z: 0.0
      orientation: 
        x: 0.0
        y: 0.0
        z: 0.417735337397
        w: 0.908568757932
---
lorenwel commented 2 years ago

You're misunderstanding. The /goal pose is subscribed to by the plan_to_goal_client.py script, which simply converts this message to an action, which is what the art_planner uses to navigate towards a goal. The art_planner publishes a nav_msgs/Path on /art_planner/path (which it looks like you're visualizing in Rviz).

Path following is usually not really the task of a planner, but I still provide a path follower script in this repo. As mentioned in the README, simply run rosrun art_planner_ros path_follower.py. You will probably need to change the Twist output topic to your own topic (presumably /anymal_c/cmd_vel).

xin111222 commented 1 year ago

Hi,I have encountered the same problem, but I don't seem to understand your solution. Can you explain to me how to modify it？ I hope to have further communication with you。

xin111222 commented 1 year ago

I used a velodyne-16 laser for mapping, but I didn't find anything around the robot, so I don't know where the problem is. Can you help me solve this problem？