MAVROS+GAZEBO: Issues about publishing lidar and flow_cam data to desired+suitable ROS topics [HELPS NEEDED URGENTLY]

ROS / MAVROS / MicroDDS
1

Hi all,

My current project is using a multicopter with 4 lidar sensors and 1 optical flow camera(pmw3901) for a wall following. I am currently using gazebo and mavros for my project. I created my own customized iris model attached with 4 lidar models and 1 flow_cam model(pmw3901) in gazebo. I want to publish all 4 lidar data to 4 ROS topics (for example: /mavros/distance_sensor/lidar0, /mavros/distance_sensor/lidar1 and etc) and publish the flow_cam data to a suitable ROS topic.

However, I am facing three main problem:

1)I am trying to use rostopic echo to check whether my lidar data were publised to 4 ROS topics, only the “/mavros/distance_sensor/lidar0” shows the data that I wanted.

rostopic echo /mavros/distance_sensor/lidar0
header: 
  seq: 1724
  stamp: 
    secs: 173
    nsecs:   9458775
  frame_id: "lidar0"
radiation_type: 1
field_of_view: 0.0
min_range: 0.0399999991059
max_range: 4.0
range: 3.34999990463

Other topics (/mavros/distance_sensor/lidar1 and etc) show me a warning which is

rostopic echo /mavros/distance_sensor/lidar1
WARNING: no messages received and simulated time is active.
Is /clock being published?

I believe that means my other distance_sensors data were not published to the topics that I wanted. Meanwhile, I also checked using Qgroundcontrol mavlink inspector and there is only 1 distance sensor data being output on the inspector.

2)I have no idea which existing topic I should use to publish pwm3901 optical flow camera data. I went through the iris_opt_flow example and the topic for px4flow camera does not fit to pmw3901. I plan to create an unique topic for pmw3901 but I am not sure where I should edit in my catkin_ws (the ws contains mavros, mavros_extra packages and etc)

3)I wrote a node cpp file to subscribe the published ROS topics (/mavros/distance_sensor/lidar1 and etc) but the result shown is always equal to 0.0.
P/S:I pasted my node cpp code below.

#include <ros/ros.h>
#include <sensor_msgs/Range.h>
//#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
//#include <mavros_msgs/CommandBool.h>
//#include <mavros_msgs/SetMode.h>
//#include <mavros_msgs/State.h>
sensor_msgs::Range current_range;
void ds_cb(const sensor_msgs::Range::ConstPtr& msg){
    current_range = *msg;
}


int main(int argc, char **argv)
{
    ros::init(argc,argv,"sensor_read_node");
    ros::NodeHandle nh;

    ros::Subscriber ds_sub =nh.subscribe<sensor_msgs::Range>("mavros/distance_sensor/lidar0",10,ds_cb);

    ROS_INFO("Current Range(Lidar): [%f]",current_range.range);
    //ROS_INFO("Vehicle armed");
    ros::spin();

}

I would love getting some help from anyone and definitely, I appreciate your time reading through my post. Thank you so much!

Regards,
ziimiin14

2

Can you attach/display the modified iris sdf model? this will be helpful

3

Hi @haritsahm,

I have attached my sdf model below.

<?xml version='1.0' ?>
<sdf version='1.6'>

    <model name='my_vehicle'>
        <include>
            <uri>model://iris</uri>
        </include>

        <!--pmw3901 optical camera facing forward-->
        <include>
            <uri>model://flow_cam</uri>
            <pose>0.1 0 0 0 0 0</pose>
            <name>pmw3901</name>
        </include>

        <joint name="opticalflow1_joint" type="revolute">
            <parent>iris::base_link</parent>
            <child>pmw3901::link</child>
            <axis>
                <xyz>0 0 1</xyz>
                <limit>
                    <upper>0</upper>
                    <lower>0</lower>
                </limit>
            </axis>
        </joint>

        <!--forward-facing lidar-->
        <include>
            <uri>model://lidar</uri>
            <pose>0.075 0.03 0.06 0 0 0</pose>
            <name>lidar0</name>
        </include>

        <joint name="lidar0_joint" type="fixed">
            <parent>iris::base_link</parent>
            <child>lidar0::link</child>
        </joint>

        <!--forward1-facing lidar-->
        <include>
            <uri>model://lidar1</uri>
            <pose>0.075 0.03 -0.06 0 0 0</pose>
            <name>lidar1</name>
        </include>

        <joint name="lidar1_joint" type="fixed">
            <parent>iris::base_link</parent>
            <child>lidar1::link</child>
        </joint>


        <!--forward2-facing lidar-->
        <include>
            <uri>model://lidar2</uri>
            <pose>0.075 -0.03 0.06 0 0 0</pose>
            <name>lidar2</name>
        </include>

        <joint name="lidar2_joint" type="fixed">
            <parent>iris::base_link</parent>
            <child>lidar2::link</child>
        </joint>

        <!--forward3-facing lidar-->
        <include>
            <uri>model://lidar3</uri>
            <pose>0.075 -0.03 -0.06 0 0 0</pose>
            <name>lidar3</name>
        </include>

        <joint name="lidar3_joint" type="fixed">
            <parent>iris::base_link</parent>
            <child>lidar3::link</child>
        </joint>



    </model>
</sdf>
4

remove all the numbering in the uri and child tag ex: model://lidar

<?xml version='1.0' ?>
<sdf version='1.6'>

    <model name='my_vehicle'>
        <include>
            <uri>model://iris</uri>
        </include>

        <!--pmw3901 optical camera facing forward-->
        <include>
            <uri>model://flow_cam</uri>
            <pose>0.1 0 0 0 0 0</pose>
            <name>pmw3901</name>
        </include>

        <joint name="opticalflow1_joint" type="revolute">
            <parent>iris::base_link</parent>
            <child>pmw3901::link</child>
            <axis>
                <xyz>0 0 1</xyz>
                <limit>
                    <upper>0</upper>
                    <lower>0</lower>
                </limit>
            </axis>
        </joint>

        <!--forward-facing lidar-->
        <include>
            <uri>model://lidar</uri>
            <pose>0.075 0.03 0.06 0 0 0</pose>
            <name>lidar0</name>
        </include>

        <joint name="lidar0_joint" type="fixed">
            <parent>iris::base_link</parent>
            <child>lidar::link</child>
        </joint>

        <!--forward1-facing lidar-->
        <include>
            <uri>model://lidar</uri>
            <pose>0.075 0.03 -0.06 0 0 0</pose>
            <name>lidar1</name>
        </include>

        <joint name="lidar1_joint" type="fixed">
            <parent>iris::base_link</parent>
            <child>lidar::link</child>
        </joint>


        <!--forward2-facing lidar-->
        <include>
            <uri>model://lidar</uri>
            <pose>0.075 -0.03 0.06 0 0 0</pose>
            <name>lidar2</name>
        </include>

        <joint name="lidar2_joint" type="fixed">
            <parent>iris::base_link</parent>
            <child>lidar::link</child>
        </joint>

        <!--forward3-facing lidar-->
        <include>
            <uri>model://lidar</uri>
            <pose>0.075 -0.03 -0.06 0 0 0</pose>
            <name>lidar3</name>
        </include>

        <joint name="lidar3_joint" type="fixed">
            <parent>iris::base_link</parent>
            <child>lidar::link</child>
        </joint>



    </model>
</sdf>
5

Hi @haritsahm,

That doesn’t work. How does the mavros topics (ex: mavros/distance_sensor/lidar0, mavros/distance_sensor/lidar1 and etc) differentiate the name if all the child tags have the same “lidar” name?

6

Sorry i haven’t tried my own suggestion. I tried to modified the gazebo_lidar_plugin so it can advertise with a specific topic name but somehow it doesn’t publish anything. You can modify something in the gazebo_lidar_plugin and sdf file

7

I did that too before I opened up a forum. I couldn’t retrieve the topic name that I advertise via the rostopic list. It seems like the gazebo_lidar_plugin in px4 firmware was not integrated with ROS.

In fact, I tried up with gazebo_ros_range plugin and actually it works. Now, I do have 4 ROS topics for lidars (range/distance sensor).

8

These two problems are solved. Please let me know if anyone wants to know the detailed approach.

Currently, I am still trying to deal with this issue. I tried using gazebo_opticalflow_plugin and gazebo_opticalflow_mockup_plugin to publish the flow_cam model data that attached to my customized iris model. However, I couldn’t see any relavant topic when I am running rostopic list. If there is anyone knows how to solve this problem, please let me know.

9

Hi @ziimiin14, I am trying to get a lidar mounted on an SDF model and I faced the same issue after using the Lidar module :

Could you help me out here? How did you get the plugins activated?

10

@Jaroan gazebo plugins are well documented in: http://gazebosim.org/tutorials?tut=ros_gzplugins

1 Like
11

@Jaeyoung-Lim Thank you for the link, I’ve understood how to add sensors and plugins. I am, however, unable to get any /mavros/distance_sensor/<sensor_name> topics via the rostopic list command.

I read that one has to remove distance_sensor from the blacklist, the plugin in the YAML configuration file Linked here. I am not knowing what are the full steps involved for this.

I have tried both the Lidar as well as the RPLidar models from the PX4 sitl_gazebo repo to be included in the vehicle SDF file with appropriate links and joints, but I was unsuccessful

12

@Jaroan I think you are expecting the plugins to automatically publish data to ROS.

Gazebo plugins by default DO NOT publish their sensor data to ROS unless it is implemented. You need to publish them from the plugins.

13

Okay, I understand. Thank you!