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

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

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

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>

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>

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?

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

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).

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.