DISTANCE_SENSOR support #292

TSC21 · 2015-05-11T18:22:34Z

Code is done! Tested the readings with a rosserial_python node which was retrieving data from an Arduino into a Range type of message, which is read by the plugin usage. So now this only lacks implementation on the FCU side, which includes receive and send and publish/advertise it a DISTANCE_SENSOR uOrb topic.

vooon · 2015-05-11T22:21:48Z

mavros_extras/src/plugins/distance_sensor.cpp

+			dist_laser_in_pub = dist_nh.advertise<mavros_extras::LaserDistanceSensor>("laser_distance", 10);
+		else if (rangefinder_type == 1)
+			dist_sonar_in_pub = dist_nh.advertise<mavros_extras::SonarDistanceSensor>("sonar_distance", 10);
+		else ROS_ERROR_NAMED("rangefinder", "Invalid rangefinder type! Valid values are 0 (laser) and 1 (ultrasound)!");


Fix log name.

Wait, what's the problem with the log name? :D

vooon · 2015-05-11T22:39:00Z

Did you used uncrustify? Also i don't understand why you need non-standard messages.

Also i think that LASER type (LidarLite) is more like INFRARED range finders, rather than LaserScan sources.
So laser branch not needed (and as i remember there already exists filter node for Range->LaserScan)

TSC21 · 2015-05-11T22:49:40Z

No I didn't. Will do it ASAP! The non-standard messages is just to keep the message definition, plus th id of the sensor, orientation and covariance.
Hmm I started with that, but then I thought that in the future someone could add a rangefinder scanner and then they could use this definition instead. But, should I keep it or move to Range instead?
filter node for Range->LaserScan? Where?

vooon · 2015-05-11T23:06:42Z

Do it in separate commit. Best in last one.
Better is move to standard Range.msg. Instead of abusing id you may dynamically add/delete publishers and subscribers (connected change). Or predefine map in params (but it little bit harder, see diagnostics aggregator)
I think that i read somewhere about similar filter, maybe i wrong.

vooon · 2015-05-11T23:09:07Z

Hmm, also i think we may ask for adding LASER range finder to Range enum.

TSC21 · 2015-05-11T23:13:52Z

Ok I will move to Range.msg then. Redefine map in params is what I want to do. Can you give an hand here?

About the Range enum, how can we ask that?

TSC21 · 2015-05-11T23:45:54Z

mavros_extras/src/plugins/distance_sensor.cpp

+	 */
+	void send_sonar_dist_sensor_data(const ros::Time &stamp, uint16_t min_distance,
+								uint16_t max_distance, uint16_t current_distance) {
+		distance_sensor(stamp.toNSec() / 1000000,


Should uas->synchronise_stamp() also be used here and how?

No, synchronize needed for data comes from FCU. To FCU send current time (in us or ms).

vooon · 2015-05-12T08:51:46Z

Asked at ros-users: http://lists.ros.org/pipermail/ros-users/2015-May/069403.html

TSC21 · 2015-05-12T13:29:13Z

@vooon changes done. Using INFRARED type for Laser for now. We'll have to wait for ROS community to implement new type for Range.msg enum.

vooon · 2015-05-12T13:56:19Z

mavros_extras/msg/DistanceSensor.msg

+
+uint8 id 			# XXX FIXME enum ??
+
+uint8 orientation 	# rad


Rad? I don't think so. Uint8 is more likely orientation enum used in params for ahrs.

Probably. Will change it then

This also should be captured in the frame_id using tf information.

vooon · 2015-05-12T14:10:58Z

I still think that new message not needed. Only one really needed data is variance.
While id, type and orientation constant (per id).

May be better do following param structure:

distance_sensor:
  sensor_topic_name:
    id: 0 # sensor id
    frame_id: some_frame
  other_sensor_topic:
    id: 1
    frame_id: other_frame
  subscribed_topic:
    subscribe: true
    id: 42
    type: 0 # we may try to guess by Range.radiation_type 
    variance: ???
    orientation: 0

Pro:

regular Range.msg, so you can use rviz plugins (not sure that it can viz it now, but i remember some demo few years ago).
different frame_id's, so you can connect your urdf model with data.
no need to filter sensors by id in client nodes

TSC21 · 2015-05-12T16:47:15Z

@vooon,

Besides the variance, we also have the orientation. So to keep just a default Range.msg, and to use the full data of the Mavlink message I purpose this:

send the variance in a Float32 message format, so to use in another node
use the received orientation so to publish the transform between the rangefinder frame and the FCU frame.

What do you think?

2.I was considering dynamically add/delete publishers, depending on the rangefinder type and its id of the data coming from the FCU. But I'm not really sure how to do it. Can you give a hand here?

TSC21 · 2015-05-12T16:54:05Z

BTW, here's the RViz representation you were talking about: http://wiki.ros.org/rviz/DisplayTypes/Range

vooon · 2015-05-12T19:25:41Z

@TSC21 You don't need to use orientation, because it already coded in frame_id + transform (i think from URDF).
But maybe add check FCU orientation == plugin config is not bad. Because orientation FIXME enum :) may differ from definition.

Maybe for now we may drop variance? Or show in diagnostics. At least until someone not need it.

I think instead of dynamic add/del better specify topics in config file (px4_config.yaml) and provide mapping to id (plus warning messages about unmapped id's).
I think that it should be easier to work with, but harder in rosparam walk.
See example https://github.com/mavlink/mavros/blob/master/mavros/src/plugins/param.cpp#L794-L835

tfoote · 2015-05-13T06:20:34Z

mavros_extras/msg/DistanceSensor.msg

+
+sensor_msgs/Range range
+
+uint8 type			# see MAV_DISTANCE_SENSOR enum


This is already in the Range message

Yes you are right. But since it won't be used, I will delete this custom msg. Anyway, it's useful info for future implementation, so thanks :)

tfoote · 2015-05-13T06:25:41Z

I agree with @vooon to not create a new message datatype. The variance is not currently captured in the Range message, but that should not be a problem to parameterize properly when processing. If we need it we can consider adding it to the Range message, however from experience it tends to be application specific more than device specific. Similarly the LaserScan does not have variance/covariance.

vooon · 2015-05-13T22:50:50Z

@TSC21 i'm done. But it little incomplete, but not that not big deal.

Example test config:

mavros:
  plugin_whitelist: ["sys_*", distance_sensor]
  distance_sensor:
    topic_pub_1:
      id: 0
      frame_id: sensor_1
    topic_pub_2:
      id: 1
      frame_id: sensor_2
      orientation: 2
    topic_sub_1:
      subscriber: true
      id: 2
      orientation: 2

But i don't tested recv/send (need FCU or emulator).

TSC21 · 2015-05-13T23:37:45Z

@vooon why have you changed all the code structure? Am I supposed to pick up what you've done a add something to it? Because from what I can see you done a new PR all by yourself and mine almost disappeared...

vooon · 2015-05-13T23:51:54Z

@TSC21 because i done param to map, which requires many instances of pub/sub
i moved part of code to item class. Then i cleaned up, so from your code now left only skeleton :)

TSC21 · 2015-05-13T23:56:45Z

OK... but then I think the colaborative part somewhat disappeared on this. I want to give some contribution but like this is like doing nothing. You can add it as your own PR then.

vooon · 2015-05-14T00:04:23Z

Sorry, but i done what i mentioned yesterday (topic-id-frame_id mapping).
Also I really don't have enough time to test, and i don't do some params (fov etc, see XXX comments).
Code now in master, you can further fix it by next PR.

TSC21 · 2015-05-14T00:10:51Z

Ok. Sent you a PM in GHangout regarding this. Anyway, I'll see what I can do about other stuff. Also I don't know why you deleted covariance publishing. We didn't agreed to drop it down.

TSC21 · 2015-05-14T13:16:03Z

@vooon, regarding this new config:
I have the following in px4_config.yaml:

distance_sensor:
  sonar_1_pub:
    id: 0
    frame_id: sonar_1
    orientation: 1
  laser_1_pub:
    id: 1
    frame_id: laser_2
    orientation: 1
  sonar_1_sub:
    subscriber: true
    id: 2
    orientation: 1

If in the .ino file I create a publisher like this: ros::Publisher pub_range("/sonar_1_sub", &range_msg); with frame_id = 2, is it suppose to subscribe? Cause mavros is not subscribing to this topic using the above configuration.
In the other way, if I use ros::Publisher pub_range("/mavros/distance_sensor/sonar_1_sub", &range_msg); then the topic subscribes, but then if I set frame_id = 1 in the Arduino code, it still subscribes, but with frame_id=1, but it shouldn't cause frame_id != id in the config, right? Or is the frame_id suppose to be independent from the id of the sensor sent to the FCU?

vooon · 2015-05-14T13:34:26Z

Subscriber not used frame_id, just use right topic (/mavros/distance_sensor/sonar_1_sub, look at rosnode info mavros).
And it should send DISTANCE_SENSOR.id=2 for any incoming message on that topic.

TSC21 · 2015-05-14T13:45:59Z

OK, understood. Adding some enhancements to code now (variance calculation, etc.). Then I'll do PR.

TSC21 · 2015-05-14T14:59:29Z

Trying to troubleshoot an error, which is not allowing me to load the mavros_extras plugins. This is what I added to px4_config.yaml:

#distance_sensor
distance_sensor:
  hrlv_ez4_pub:
    id: 0
    frame_id: "hrlv_ez4_sonar"
    orientation: 1      # Default for Maxbotix HRLV-EZ4; in meters
    min_range: 0.3
    max_range: 5.0
    field_of_view: 0.0  # XXX TODO
  lidarlite_pub:
    id: 1
    frame_id: "lidarlite_laser"
    orientation: 1      # Default for PulsedLight LIDARLite; in meters
    min_range: 0.0
    max_range: 40.0
    field_of_view: 0.0  # XXX TODO
  sonar_1_sub:
    subscriber: true
    id: 2
    orientation: 1
  laser_1_sub:
    subscriber: true
    id: 3
    orientation: 1

(I put the min/max range and fov as needed param for pub).

I get this when launching the node:

[ERROR] [1431615369.006175623]: Plugin distance_sensor load exception: Failed to load library /home/nuno/AIMAV_Project/devel/lib//libmavros_extras.so. Make sure that you are calling the PLUGINLIB_EXPORT_CLASS macro in the library code, and that names are consistent between this macro and your XML. Error string: Could not load library (Poco exception = /home/nuno/AIMAV_Project/devel/lib//libmavros_extras.so: undefined symbol: _ZN9mavplugin20DistanceSensorPlugin8readingsE)
[ERROR] [1431615369.113900603]: Plugin mocap_pose_estimate load exception: MultiLibraryClassLoader: Could not create object of class type mavplugin::MocapPoseEstimatePlugin as no factory exists for it. Make sure that the library exists and was explicitly loaded through MultiLibraryClassLoader::loadLibrary()
[ERROR] [1431615369.166365302]: Plugin px4flow load exception: MultiLibraryClassLoader: Could not create object of class type mavplugin::PX4FlowPlugin as no factory exists for it. Make sure that the library exists and was explicitly loaded through MultiLibraryClassLoader::loadLibrary()
[ERROR] [1431615369.305468753]: Plugin vision_pose_estimate load exception: MultiLibraryClassLoader: Could not create object of class type mavplugin::VisionPoseEstimatePlugin as no factory exists for it. Make sure that the library exists and was explicitly loaded through MultiLibraryClassLoader::loadLibrary()
[ERROR] [1431615369.359173584]: Plugin vision_speed_estimate load exception: MultiLibraryClassLoader: Could not create object of class type mavplugin::VisionSpeedEstimatePlugin as no factory exists for it. Make sure that the library exists and was explicitly loaded through MultiLibraryClassLoader::loadLibrary()
[ERROR] [1431615369.411541570]: Plugin visualization load exception: MultiLibraryClassLoader: Could not create object of class type mavplugin::VisualizationPlugin as no factory exists for it. Make sure that the library exists and was explicitly loaded through MultiLibraryClassLoader::loadLibrary()

Solutions?

mhkabir · 2015-05-14T16:05:38Z

Small note : The PX4Flow sonar doesn't publish to the PX4 distance_sensor uORB topic (Hence no mavlink message sent) . There will be no output for it.
The LL040S, SF0x, etc. do. The PX4Flow plugin already handles the sonar publishing. ;)

Remove the HRLV-EZ4 please, or follow up with a PR to Firmware which publishes the sonar to the distance sensor topic and also the required modifications to all the estimators to use the topic.

TSC21 · 2015-05-14T16:12:42Z

Small note : The PX4Flow sonar doesn't publish to the PX4 distance_sensor uORB topic (Hence no mavlink message sent) . There will be no output for it.
The LL040S, SF0x, etc. do. The PX4Flow plugin already handles the sonar publishing. ;)

I'm aware of it thanks ;)

follow up with a PR to Firmware which publishes the sonar to the distance sensor topic and also the required modifications to all the estimators to use the topic.

It's my next step. Also I'm planning to create a .h with various sensor definitions to be used in the ROS config.

Anyway, @mhkabir do you have any idea of what can be firing the errors above?

vooon · 2015-05-14T16:57:28Z

@TSC21 try catkin clean --all && catkin build

TSC21 · 2015-05-14T17:01:02Z

catkin clean --all && catkin build is not recognized. But doing catkin_make clean and then catkin_make didn't help.

TSC21 · 2015-05-14T17:11:03Z

OK I installed catkin build tools (apt-get install python-catkin-tools) so to use the above commands. But after compiling everything I still get the same errors!

TSC21 · 2015-05-14T17:30:28Z

Well, it is solved. Had to clone repo again and add changes. Will do a PR now.

Update: nope! Apparently not. I did a clean and build again and it show this errors again. Really don't know the problem at all. Will it help send a PR so you can check the error, @vooon?

vooon · 2015-05-14T21:31:17Z

Commit changes to some branch.

TSC21 · 2015-05-15T05:02:56Z

@vooon here it is: f5c4113.

vooon · 2015-05-15T11:03:23Z

Commented in diff. Main problem source is static class members you added. Why you ever need static?

TSC21 added 2 commits May 11, 2015 18:46

distance_sensor plugin: first commit!

db91dce

distance_sensor: add plugin file

7baa84a

vooon reviewed May 11, 2015
View reviewed changes

TSC21 reviewed May 11, 2015
View reviewed changes

distance_sensor: minor fixes that include use Range.msg to Laser data

4bb3f1e

TSC21 added 2 commits May 12, 2015 14:34

distance_sensor: minor correction

90133d6

distance_sensor: comment correction

d5bd2c8

vooon reviewed May 12, 2015
View reviewed changes

tfoote reviewed May 13, 2015
View reviewed changes

vooon added a commit that referenced this pull request May 13, 2015

distance_sensor #292: implement message handling

87e17a5

vooon added a commit that referenced this pull request May 13, 2015

distance_sensor #292: fix travis build.

036fbb6

vooon added this to the Version 0.12 milestone May 13, 2015

vooon added a commit that referenced this pull request May 13, 2015

distance_sensor #292: uncrustify

38db24d

vooon mentioned this pull request May 13, 2015

Add support to DISTANCE_SENSOR msg coder/decoder #264

Closed

TSC21 mentioned this pull request Jun 21, 2017

Distance sensors improvements #740

Closed


		sensor_msgs/Range range

		uint8 type # see MAV_DISTANCE_SENSOR enum

DISTANCE_SENSOR support #292

DISTANCE_SENSOR support #292

Conversation

TSC21 commented May 11, 2015

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

vooon commented May 11, 2015

TSC21 commented May 11, 2015

vooon commented May 11, 2015

vooon commented May 11, 2015

TSC21 commented May 11, 2015

Choose a reason for hiding this comment

Choose a reason for hiding this comment

vooon commented May 12, 2015

TSC21 commented May 12, 2015

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

vooon commented May 12, 2015

TSC21 commented May 12, 2015

TSC21 commented May 12, 2015

vooon commented May 12, 2015

Choose a reason for hiding this comment

Choose a reason for hiding this comment

tfoote commented May 13, 2015

vooon commented May 13, 2015

TSC21 commented May 13, 2015

vooon commented May 13, 2015

TSC21 commented May 13, 2015

vooon commented May 14, 2015

TSC21 commented May 14, 2015

TSC21 commented May 14, 2015

vooon commented May 14, 2015

TSC21 commented May 14, 2015

TSC21 commented May 14, 2015

mhkabir commented May 14, 2015

TSC21 commented May 14, 2015

vooon commented May 14, 2015

TSC21 commented May 14, 2015

TSC21 commented May 14, 2015

TSC21 commented May 14, 2015

vooon commented May 14, 2015

TSC21 commented May 15, 2015

vooon commented May 15, 2015