search

open-rmf / rmf_obstacle

Packages that infer the presence of obstacles from sensor inputs.
Apache License 2.0
6 stars 10 forks source link

[Other issue]: Simulate laser scan in Gazebo #23

Closed Jerrybaoyilei closed 6 months ago

Jerrybaoyilei commented 6 months ago

Before proceeding, is there an existing issue or discussion for this?

Description

Hi @Yadunund , I am quite new to ROS 2 and Open-RMF.

I was trying to implement the rmf_obstacle_detector_laserscan package into the rmf_demos package. I also followed your instructions in #15 . Now in my terminal I can see "[INFO] [1711031152.446753857] [laserscan_obstacle_detector]: Activation successful. Waiting to receive 10 LaserScan messages on topic lidar/scan to begin calibration.", but since I don't have any simulated laser scanner in my gazebo simulation, there is currently no LaserScan message published to this lidar/scan topic.

I wonder which simulated laser scanner package did you use in achieving the deep blue laser scan result in the gif you showed in Readme? I've checked in Gazebo Classic tutorials, and the laser scan plugin are attached to a robot, whereas the one in this gif looks like a stationary one, which is what I hope to use. Thank you very much!

Yadunund commented 6 months ago

Hi @Jerrybaoyilei,

You'll need to add a lidar model to your .world file or spawn it into the world. ie, just as you would attach a lidar model to one of your robot links through a fixed joint, you could need to instead create a fixed joint between the model and the world. Hope that clarifies. Since this is not a bug i'll close this ticket but feel free to reopen otherwise. If you're planning to submit a GSoC proposal, there is a line item in #22 related to rmf_demos where the goal would be to spawn such sensors in a demo world as part of a end-to-end simulation of the obstacle detection pipeline.

Jerrybaoyilei commented 6 months ago

Hi @Yadunund , thank you for the clarification! From my understanding, attaching a lidar model to the robot won't work with this package, right? Since the "obstacle-free" basis world is created from the first 10 LaserScan messages (please correct me if I am wrong)

I am not submitting a GSoC proposal. I am just exploring open-rmf and trying out the simulations (thank you and your team for building this amazing platform)

Also, I apologize that I asked the question here even though it's not a bug. It's just I couldn't find much information online regarding this package, and I figured maybe I could ask here directly...

Yadunund commented 6 months ago

Yup you don't need to attach the model to the robot. Instead attach it to the world so it remains stationary in space.

Jerrybaoyilei commented 6 months ago

Hi @Yadunund , I used the ros_gz_bridge you mentioned in the other ticket. Then I realized that it no longer works with Gazebo Classic (which is what I've been using). Therefore, I had to reconstruct the LiDAR model for Ignition Gazebo 6 (Fortress) based on this Gazebo tutorial

However, even though I successfully got some data reading from echoing the converted ROS 2 topics, the LiDAR has an intensity of 0 and I cannot see any rays in Ignition Gazebo. I wonder if you used some version of the ros_gz_bridge that works with Gazebo classic and ROS 2? Since the gif in the readme of this package looks like Gazebo classic. Thank you!

Yadunund commented 6 months ago

It's best to stick to Gazebo (new) as Gazebo Classic (old) will be retired soon. With the new Gazebo, you'll need to explictly add a plugin in the world sdf to visualize the lidar rays. See https://github.com/gazebosim/gz-sim/blob/b9138fb12b0cef77621ed3a5eba577ebe4f7abd0/examples/worlds/visualize_lidar.sdf#L125-L126

Regarding intensities, you'll need to check whether the lidar plugin in Gazebo supports this. Even if it did, I assume it would be conditioned on every asset in the gazebo world properly defining PBR parameters. https://github.com/gazebosim/gz-sensors/blob/gz-sensors8/include/gz/sensors/GpuLidarSensor.hh

Usually, relying on the range values is sufficient.

Jerrybaoyilei commented 6 months ago

@Yadunund Got it. Thank you!

Also, is there a historical version of this package (rmf_obstacle) that works with Humble binaries of ROS 2 and rmf that I can use? I am asking only because I modified some code when I was using ROS 2 Humble and the humble binary of rmf, and some of these packages were significantly changed in ROS 2 Iron. Thank you!

Yadunund commented 6 months ago

There isn't. Since this package is not stabilized yet, there are no distro branches associated with it.

Jerrybaoyilei commented 5 months ago

Got it, thank you!

Jerrybaoyilei commented 5 months ago

Hi @Yadunund , sorry to bother you with yet another question. I got the LiDAR showing up in GUI and laser detector working (I can see the terminal output indicating there are obstacles detected). But when I tried to run ros2 run rmf_obstacle_ros2 lane_blocker_node and then add an obstacle in Ignition Gazebo 6, I got the following in the terminal:

[INFO] [1712633404.831764106] [lane_blocker_node]: Message Filter dropping message: frame 'VelodyneHDL-32/top/gpu_lidar' at time 0.000 for reason 'the timestamp on the message is earlier than all the data in the transform cache'

Have you encountered this before? I am using ROS 2 Iron (binary installation) and rmf iron-release branch (binary installation)

Yadunund commented 5 months ago

Are you piping the /clock topic via the ros_gz_bridge node? This is to synchronize th time between the Gz simulation and the ROS nodes, else the latter will be using the current system time.

Jerrybaoyilei commented 5 months ago

@Yadunund yes, I ran ros2 run ros_gz_bridge parameter_bridge /clock@rosgraph_msgs/msg/Clock@ignition.msgs.Clock in a new terminal, but still got the same output complaining about "timestamp on the message earlier than all the data in the transform cache".

After outputing the tf frame graph before and after running the lane blocker node, I noticed that I don't have a frame for the lidar sensor; the only frames are for tinyRobot1 and tinyRobot2 in the demo office simulation. Could this lack of frame be the reason for all the dropping message?

In my office.world, I added <plugin filename="libignition-gazebo-sensors-system.so" name="ignition::gazebo::systems::Sensors" /> inside the <world> tag; should I maybe add a <frame_name> tag inside this <plugin> tag? I've attached my office.world, model.sdf for the lidar for reference. Thank you! office world and lidar model sdf.zip

Yadunund commented 5 months ago

Did you pass use_sim_time: True as ROS parameter to all your nodes?

Jerrybaoyilei commented 5 months ago

Hi @Yadunund , no I didn't, not all nodes. I just changed the default value of use_sim_time to True in launch files in rmf_demos, but I discovered that some nodes still have use_sim_time: False, for example, coke ingestor

I now added a transform broadcaster to broadcast the location of lidar model frame to the world frame, and the error is gone. However, the lane is still not closed. Seems like the lane blocker is able to recognize that the obstacle is on the robot lane though. Here is part of the output from the lane_blocker node terminal:

================================================
Checking collision between: 
Lane: {18.767,-8.62319,1.55511} [3.49886,0.5]
obs: {18.7976,-8.03186,-1.5708} [0.976989,-0.976989]
Obs rot matrix: 
2.83277e-16           1
         -1 2.83277e-16
Obs vertices before trans: {18.3091,-7.54337} {19.2861,-7.54337} {18.3091,-8.52035} {19.2861,-8.52035} 
Lane rot inv matrix: 
0.0156901  0.999877
-0.999877 0.0156901
Obs vertices after trans: {1.07251,0.474788} {1.08784,-0.50208} {0.0956431,0.459459} {0.110972,-0.517409} 
Lane box_min: -1.74943,-0.25
Lane box_max: 1.74943,0.25
obs transformed box_min: 0.0956431,-0.517409
obs transformed box_max: 1.08784,0.474788
[INFO] [1712780669.766379482] [lane_blocker_node]: Obstacle rmf_obstacle_detector_laserscan_0 is still in the vicinity of lane tinyRobot_42
================================================
Checking collision between: 
Lane: {18.767,-8.62319,1.55511} [3.49886,0.5]
obs: {18.7976,-8.03186,-1.5708} [0.976989,-0.976989]
Obs rot matrix: 
2.83277e-16           1
         -1 2.83277e-16
Obs vertices before trans: {18.3091,-7.54337} {19.2861,-7.54337} {18.3091,-8.52035} {19.2861,-8.52035} 
Lane rot inv matrix: 
0.0156901  0.999877
-0.999877 0.0156901
Obs vertices after trans: {1.07251,0.474788} {1.08784,-0.50208} {0.0956431,0.459459} {0.110972,-0.517409} 
Lane box_min: -1.74943,-0.25
Lane box_max: 1.74943,0.25
obs transformed box_min: 0.0956431,-0.517409
obs transformed box_max: 1.08784,0.474788
[INFO] [1712780669.767033042] [lane_blocker_node]: Obstacle rmf_obstacle_detector_laserscan_0 is still in the vicinity of lane tinyRobot_43
[INFO] [1712780669.767117245] [lane_blocker_node]: There are 0 lanes with changes to the number of obstacles in their vicinity
[INFO] [1712780670.764610955] [lane_blocker_node]: Obstacle rmf_obstacle_detector_laserscan_0 was previously in the vicinity of 2 lanes
================================================

The rest of the output is repeating these 2 blocks of output.

The entire time, I placed the obstacle there and never moved it away.

I also checked the rqt_graph and compared with https://github.com/open-rmf/rmf_obstacle/issues/9#issuecomment-1411453963. I've attached the .dot file as well. In the rqt graph, the tinyRobot_command_handle doesn't seem to be publishing to /closed_lanes like the rqt graph in that ticket comment. Lane_blocker also only has one output /lane_closure_requests and missing /speed_limit_requests. Is there something I am missing here? Many thanks!

image rosgraph.zip