costmap_generator work,but it topic /semantics/costmap_generator/occupancy_grid do not have data!!!
i launch the costmapgenerator,i have launch the need topic!!! i debug the topic /semantics/costmapgenerator/occupancygrid do not have data!!! it message data[] all 0 ,it lead to astarnavi have previous warn! Invalid start pose!
Asked by rosli on 2020-12-06 08:53:29 UTC
Answers
Hi @rosli an Update of my previous answer with the solution: Here to send the solution for some users that faced the same issue.
The tutorials that I have followed and described above in my question are WRONG! If you launch the costmap_generator through the runtime manager or terminal, the occupancy grid map will not be generated or very very rarely....
To fix this there are 2 options:
1). Using Run-time Manager: on Computing Tab click over app option in costmap_generator, scroll down until you find the Lanelet2 checkbox option at the left bottom. CHECK this box ✔️
2) Inside the docker, using a new terminal launch:
roslaunch costmap_generator costmap_generator_lanelet2.launch
Ok now the costmap_generator will generate the occupancy_grid_map 3) Just Pay attention that you must check the ray_ground filter or compare_map_filter on Sensing Tab
However this will not help as the A* keeps not avoiding and crashing the Obstacle, as described in my issue: #2387
If someone fix this A* please let me know
Hi before reading after the dashed line below, check this video I have recorded:
https://www.youtube.com/watch?v=9gbq60B6Yb0&list=PL_d_YiRmUA3nnTRmF8QJKyPoEQhrdvpP2&index=20
I have followed the step-by-step from this documentation, to generate the occupancy_grid map:
The issue here is that the Perception module on Autoware.AI project is not robust (Fails 8 from 10 attempts to use it) Another issue is that this module requires a very high computational power. I was not able to use it on my notebook with the LG simulator. Then I needed to rent a very powerful/ cloud machine on paperspace.com or run a distributed simulation (desktop/notebook):
If you run in powerful machines the Planner will receive the data from the semantics/costmap_generator_occupancy_grid topic.
However as I said the module is not robust and will fail a lot. Also to update and generate new obstacles, while the car moves, into the occupancy grid. It does not get update...(you can check this behaviour in youtube video link above).
A possible reason for this, is that each revolution of velodyne get 1.200.000 points and need to be very correctly filtered as this specialist suggest in his course:
If you be interested how I debugged this error from the beginning read the lines below:
Hi is this in Autoware.AI?
Did you figure out how to get the data?
I suspect that the laser scan is the responsible to get this data and publish on occupancy grid map, according to http://wiki.ros.org/gmapping:
And if we inspect the topic nav_msgs/OccupancyGrid. We can check that it is really not subscribing to any LaserScan data, or better there is not any LaserScan topic publisher, publishing into nav_msgs/OccupancyGrid:
$ rostopic info /semantics/costmap_generator/occupancy_grid
Type; nav_msgs/OccupancyGrid
Publishers: None
Subscribers:
* /rviz_1619807356229344627 (http://local-host:38799/)
I was taking a look into the file shared_dir/autoware-data/BorregasAvemy_launch/my_sensing_simulator.launch :
<launch>
<!-- ROS-Bridge node for Simulator connection -->
<node name="websocket_bridge" pkg="rosbridge_server" type="rosbridge_websocket" output="screen" clear_params="true" required="true" />
<!-- Re-publishing simulator/camera_node/image/compressed topic to /image_raw as expected by Autoware -->
<node name="republish" type="republish" pkg="image_transport" output="screen" args="compressed in:=/simulator/camera_node/image raw out:=/image_raw" />
<!-- FIXME: this is to make a temporary hard-link TF between /gps (GPS coordinates fix) and /base_link (base of the car) if localization is failed (e.g. no lidar data) -->
<!-- <node pkg="tf" type="static_transform_publisher" name="gps_to_base_link" args="0 0 0 0 0 0 /gps /base_link 10"/> -->
<!-- <node pkg="tf" type="static_transform_publisher" name="lidar_to_camera" args="0.029 -0.574 -1.820 4.712 0.009 -1.883 /velodyne /camera 10" /> -->
</launch>
This file is the responsible to establish the brige connection between SVL simulator and Autoware. In its content, we can see that just the camera image is subscribed through the package image_transport. I have the feeling that just the camera image is not enough to update the occupancy_grid_map. For this reason I believe that in this "my_sensing_simulator.launch" a new line of code must be inserted subscribing sensor_msgs/LaserScan topic, as the gmapping documentation above explains
Adapting to the SVL used LaserScan to get data, I guess that in this line of code to be added, we must subscribe to point_cloud/velodyne topic instead: https://www.svlsimulator.com/docs/simulation-content/sensors-list/#lidar
Regarding the line of code to be added, I am not sure which package of LaserScan data to use, among the ones available in ros-perception: https://github.com/ros-perception. Maybe get raw data using:
- laser_geometry
- laser_filters
- perception_pcl
Or it will be necessary to convert the camera image or laser data to point_cloud and use one of the pkgs below:
- depthimage_to_laserscan
- pointcloud_to_laser_scan
And to improve the laser data or even make this subscription work. May be necessary to use auxiliar packages, such as:
- laser_pipeline
- laser_assembler
- pcl_msgs
- pcl_conversions
What let me worried is that the topic nav_msgs/OccupancyGrid does not accept 3D PCL data, but just 2D LaserScan data type. Then probably we will need a launch file that calls the package point_cloud_to_laserscan:
http://wiki.ros.org/pointcloud_to_laserscan
Then the occupancy_grid_map must be updated and the Nav_A_star motion planner algorithm should be triggered and finally work to avoid obstacles:
<launch>
<!-- Relay behavior configurations -->
<arg name="safety_waypoints_size" default="100" />
<arg name="update_rate" default="10" />
<!-- Avoidance behavior configurations -->
<arg name="costmap_topic" default="semantics/costmap_generator/occupancy_grid" />
<arg name="enable_avoidance" default="false" />
<arg name="avoid_waypoints_velocity" default="10.0" />
<arg name="avoid_start_velocity" default="3.0" />
<arg name="replan_interval" default="2.0" />
<arg name="search_waypoints_size" default="50" />
<arg name="search_waypoints_delta" default="2" />
<arg name="closest_search_size" default="30" />
<!-- A* search configurations -->
<arg name="use_back" default="false" />
<arg name="use_potential_heuristic" default="true" />
<arg name="use_wavefront_heuristic" default="false" />
<arg name="time_limit" default="1000.0" />
<arg name="robot_length" default="4.5" />
<arg name="robot_width" default="1.75" />
<arg name="robot_base2back" default="1.0" />
<arg name="minimum_turning_radius" default="6.0" />
<arg name="theta_size" default="48" />
<arg name="curve_weight" default="1.2" />
<arg name="reverse_weight" default="2.00" />
<arg name="lateral_goal_range" default="0.5" />
<arg name="longitudinal_goal_range" default="2.0" />
<arg name="angle_goal_range" default="6.0" />
<arg name="obstacle_threshold" default="100" />
<arg name="potential_weight" default="10.0" />
<arg name="distance_heuristic_weight" default="1.0" />
<node pkg="waypoint_planner" type="astar_avoid" name="astar_avoid" output="screen">
<param name="safety_waypoints_size" value="$(arg safety_waypoints_size)" />
<param name="update_rate" value="$(arg update_rate)" />
<remap from="costmap" to="$(arg costmap_topic)" />
<param name="enable_avoidance" value="$(arg enable_avoidance)" />
<param name="search_waypoints_size" value="$(arg search_waypoints_size)" />
<param name="search_waypoints_delta" value="$(arg search_waypoints_delta)" />
<param name="closest_search_size" value="$(arg closest_search_size)" />
<param name="avoid_waypoints_velocity" value="$(arg avoid_waypoints_velocity)" />
<param name="avoid_start_velocity" value="$(arg avoid_start_velocity)" />
<param name="replan_interval" value="$(arg replan_interval)" />
<param name="use_back" value="$(arg use_back)" />
<param name="use_potential_heuristic" value="$(arg use_potential_heuristic)" />
<param name="use_wavefront_heuristic" value="$(arg use_wavefront_heuristic)" />
<param name="time_limit" value="$(arg time_limit)" />
<param name="robot_length" value="$(arg robot_length)" />
<param name="robot_width" value="$(arg robot_width)" />
<param name="robot_base2back" value="$(arg robot_base2back)" />
<param name="minimum_turning_radius" value="$(arg minimum_turning_radius)" />
<param name="theta_size" value="$(arg theta_size)" />
<param name="angle_goal_range" value="$(arg angle_goal_range)" />
<param name="curve_weight" value="$(arg curve_weight)" />
<param name="reverse_weight" value="$(arg reverse_weight)" />
<param name="lateral_goal_range" value="$(arg lateral_goal_range)" />
<param name="longitudinal_goal_range" value="$(arg longitudinal_goal_range)" />
<param name="obstacle_threshold" value="$(arg obstacle_threshold)" />
<param name="potential_weight" value="$(arg potential_weight)" />
<param name="distance_heuristic_weight" value="$(arg distance_heuristic_weight)" />
</node>
</launch>
I am currently facing the same issue: https://answers.ros.org/question/377308/autowareai-perception-failure-agent-not-detected-in-rviz/
I have advanced to solve this issue (90% solved) . THE ANSWER IS IN THE LINK ABOVE.
So please if you have figured out how to solve this issue. Please let me know. I will save a lot of effort...hehehehe
Asked by Vini71 on 2021-04-30 11:46:03 UTC
Comments
Please don't use screenshots for text (e.g., code, terminal output, etc.) as people can't copy/paste the text from them and screenshots are actually (in my experience) harder to read than text (text also takes less bandwidth). Can you please update your answer with the text copied and pasted instead of using images?
Asked by jayess on 2021-04-30 22:51:46 UTC
ok I have updated. Sometimes I prefer to use screenshot because the context is not providethe solution as code to be copied. But just display where, or better in which line of code the issue is related to....I think is more intuitive for the user just take a look and understand. Also because taking screenshot from terminal or text editors, I can bold or mark the relevant lines of code that the ROS developer should focus his/her attention.
Asked by Vini71 on 2021-05-01 10:04:19 UTC
Comments
hey @rosli I have updated my question. I believe I have found the answer from your doubt.Could you please close it if you accept it? Thanks :)
Asked by Vini71 on 2021-05-18 10:32:24 UTC