[SOLVED] Accessing various information through ROS

[Che.]

Hi,

some of you know, that I am working on a project, where I use the crazyflie to build a 3D-Map.
I am quite far progressed at the moment, my simulation already works.

Now I only need to implement the sensor readings of the crazyflie into my ROS-Construct.

Is there an API for all ros-related crazyflie-topics? I need the yaw of the flie, my distance readings, that I've logged, and the LPS-3D-Coordinates.

Can you tell me how to quickly and most possible elegant access this data in ROS? Is it even possible to access the data, I've been logging in the cfclient over ROS-Topics? I am shure there is a way

Thanks for the support.

Che.

[whoenig]

Some of the standard topics (e.g. IMU data) are natively supported in the ROS driver. In your case, I would suggest using custom log blocks. You can find an example here: https://github.com/whoenig/crazyflie_ro ... cks.launch
You will just get an array of floating point values as ROS topic. If you want it "nicer", you would need to change the source code of the crazyflie_ros package and add a custom message type and code in the crazyflie_server.cpp to fill in that topic.

If you run into any issues you can ask here, or open an issue on github and I'll try to help you out.

Wolfgang

[Che.]

Hi Wolfgang and all interested readers,

my plan is, to merge distance-measurement of a lidar with the 'x,y,z,roll,pitch,yaw'-data which I use to create a variable coordinate-transformation (tf).

I followed your advice and wrote a launch file with custom log blocks.

Code: Select all

<?xml version="1.0"?>

<launch>
  <!-- LPS Part -->
  <rosparam command="load" file="$(find bitcraze_lps_estimator)/data/anchor_pos.yaml" />

  <node name="log_range" pkg="bitcraze_lps_estimator" type="log_range.py" />
  <node name="lps_pf_modified" pkg="bitcraze_lps_estimator" type="lps_pf_modified.py" />

  <!-- crazyflie part -->
  <arg name="uri" default="radio://0/80/250K" />

  <include file="$(find crazyflie_driver)/launch/crazyflie_server.launch">
  </include>

  <group ns="crazyflie">

    <node pkg="crazyflie_driver" type="crazyflie_add" name="crazyflie_add" output="screen">
      <param name="uri" value="$(arg uri)" />
      <param name="tf_prefix" value="crazyflie" />
      <rosparam>
        genericLogTopics: ["log_distance", "log_orientation", "log_ranges"]
        genericLogTopicFrequencies: [100, 100, 100]
        genericLogTopic_log_distance_Variables: ["distance_cm.d_cm"]
        genericLogTopic_log_orientation_Variables: ["mag.x", "mag.y", "mag.z"]
        genericLogTopic_log_ranges_Variables: ["ranging.distance1", "ranging.distance2", "ranging.distance3", "ranging.distance4", "ranging.distance5", "ranging.distance6", "ranging.state"]
      </rosparam>
    </node>

  </group>

    <!-- merge part -->
    <node name="pos_to_tf_converter" pkg="merge_pos_range_to_tf_pcl2" type="pos_to_tf_converter.py" output="screen" />
    <node name="rviz" pkg="rviz" type="rviz"
        args="-d $(find merge_pos_range_to_tf_pcl2)/data/rvizconfig.rviz" />
    <node name="rqt_graph" pkg="rqt_graph" type="rqt_graph" />
    <node name="range_to_pcl2_converter" pkg="merge_pos_range_to_tf_pcl2" type="range_to_pcl2_converter.py" output="screen" />

    <!-- octomap Launch part -->
	<node pkg="octomap_server" type="octomap_server_node" name="octomap_server">
		<param name="resolution" value="0.05" />
		
		<!-- fixed map frame (set to 'map' if SLAM or localization running!) -->
		<param name="frame_id" type="string" value="my_fixed_frame" />
		
		<!-- maximum range to integrate (speedup!) -->
		<param name="sensor_model/max_range" value="5.0" />

        <!-- latch -->
        <param name="latch" type="bool" value="false" />
		
		<!-- data source to integrate (PointCloud2) -->
		<remap from="cloud_in" to="/narrow_stereo/points_filtered2" />
	    
	</node>
</launch>

Getting my information from the IMU (roll, pitch, yaw) of the Crazyflie and merge that with my lidar-measurements works pretty good.
However I am now struggeling to get the x,y,z position into the node, that should merge it with the orientation to get the tf information.
I attached a Screenshot at the bottom to get you a better overview of my topics.
The problem at the moment is, that the tf.broadcaster needs the x,y,z and pitch, yaw, roll at the same time, preferably synchronized.
Searching the web, got me to message-filters in ROS. A special filter is the Time Synchronizer, see point 4.

The code of my position-to-tf-converter follows.

Code: Select all

#!/usr/bin/env python  
import rospy
import tf
import math
import sys
import message_filters

from std_msgs.msg import Int32
from merge_pos_range_to_tf_pcl2.msg import GenericLogData
from geometry_msgs.msg import Point

# Node to receive 3D-Positioning-Information of Bitcraze LPS and convert to tf-information.
# Get the 3D-Points and calculate a transformation from the moving frame(drone) to a fixed frame(LPS-Room) 

if __name__ == '__main__':

    rospy.init_node('pos_to_tf_converter')

    #init TransformBroadcaster
    br = tf.TransformBroadcaster()
        
    #init variables
    x = 0
    y = 0
    z = 0
    pitch = 0
    yaw = 0
    roll = 0

    ###
    def callback(orientation, position):
        # write the received messages to variables, then send transform message    
        x = position.x
        y = position.y
        z = position.z
        pitch = orientation.values[0] * (2 * math.pi)
        yaw = orientation.values[1] * (2 * math.pi)
        roll = orientation.values[2] * (2 * math.pi)
        #...
        br.sendTransform((x, y, z), tf.transformations.quaternion_from_euler(roll, pitch, yaw),
                     rospy.Time(0),
                     "my_moving_frame",
                     "my_fixed_frame")
    # Init node as subscriber to the corresponding topic from custom logging (see launch file LPS dwm-loc.launch, bitcraze forum for example)
    orientation_sub = message_filters.Subscriber('crazyflie/log_orientation', GenericLogData)
    position_sub = message_filters.Subscriber('crazyflie_position', Point)
    ts = message_filters.TimeSynchronizer([orientation_sub, position_sub], 10)
    ts.registerCallback(callback)
    # Keep Node alive
    rospy.spin()

This is the error message I get.

Code: Select all

[ERROR] [WallTime: 1463586280.766590] bad callback: <bound method Subscriber.callback of <message_filters.Subscriber instance at 0x7f1e01a6ebd8>>
Traceback (most recent call last):
  File "/opt/ros/jade/lib/python2.7/dist-packages/rospy/topics.py", line 720, in _invoke_callback
    cb(msg)
  File "/opt/ros/jade/lib/python2.7/dist-packages/message_filters/__init__.py", line 74, in callback
    self.signalMessage(msg)
  File "/opt/ros/jade/lib/python2.7/dist-packages/message_filters/__init__.py", line 56, in signalMessage
    cb(*(msg + args))
  File "/opt/ros/jade/lib/python2.7/dist-packages/message_filters/__init__.py", line 190, in add
    my_queue[msg.header.stamp] = msg
AttributeError: 'Point' object has no attribute 'header'

Thanks for reading so far, and for any tips!

[whoenig]

Hi Che,

A few points:
* genericLogTopicFrequencies should most likely a lower number (frequency is badly chosen here; it is the time in ms; hence 100 means 10 Hz only)
* Rather than using raw IMU data, try stabilizer.{roll,pitch,yaw} logging variables. Those use the integrated filter running at 250Hz and will give you a much better estimate. To save bandwidth, you might need to disable the IMU data collection in the ROS driver (required recompiling currently).
* Time synchronization should actually barely matter at this point, since your messages will be very close together in time anyways (and other accumulated errors will most likely be worse than what you introduce by the time being slightly off); You can just keep the latest roll/pitch/yaw reading around and use that whenever you get an updated ranging measurement.
* For the error you are seeing, it looks like you should use PointStamped rather than Point for position_sub, otherwise there is no timestamp for your message filter. However, I don't think the message_filter really does what you need. The documentation says

[...]and outputs them only if it has received a message on each of those sources with the same timestamp.

In practice, it is unlikely that two different sources will have exactly the same timestamp. I haven't used it myself though, so I might be wrong in this regard.

I hope that helps!

Wolfgang

[Che.]

Hi Wolfgang,
thanks for your reply. These hints are what I am looking for.

* Rather than using raw IMU data, try stabilizer.{roll,pitch,yaw} logging variables. Those use the integrated filter running at 250Hz and will give you a much better estimate. To save bandwidth, you might need to disable the IMU data collection in the ROS driver (required recompiling currently).

This is a nice hint, I am using stabilizer now, much better output. Arnaud already stated that to me in an email though, just forgot about it

* You can just keep the latest roll/pitch/yaw reading around and use that whenever you get an updated ranging measurement.

Here I am still meeting some difficulties.

I just wrote two seperate subscribers, but saving the stabilizer output does not seem to work globally. I want to do that because I need to send my tf in either the first or second callback. My tf output still stays at the value I initialized the variables with.

Heres my code:

Code: Select all

x = 0.0
    y = 0.0
    z = 0.0
    pitch = 90
    yaw = 0
    roll = 0

    ###
    def callback_o(msg):
        # write the received messages to the orientation variables, and keep them   
        pitch = msg.values[0]
        yaw = msg.values[1]
        roll = msg.values[2]
        rospy.loginfo("received pitch = %f orientation information!", pitch) # actually gives the correct up to date value
    def callback_p(msg):
        # write the received messages to the coordinate variables and merge them with the saved orientation values to a transform (tf)
        x = msg.x
        y = msg.y
        z = msg.z
        #rospy.loginfo("received position information!")
        br.sendTransform((x, y, z), tf.transformations.quaternion_from_euler(roll, pitch, yaw),
                     rospy.Time(0),
                     "my_moving_frame",
                     "my_fixed_frame")
	#sendTransform only sends the 90 degrees value I initialized it with above....
    # Init node as double subscriber to the corresponding topics from custom logging (see launch file LPS dwm-loc.launch, bitcraze forum for example)
    orientation_sub = rospy.Subscriber('crazyflie/log_orientation', GenericLogData, callback_o)
    position_sub = rospy.Subscriber('crazyflie_position', Point, callback_p)
    # Keep Node alive
    rospy.spin()

I could not quite find something about static variables in ros so far. Is this even the right way? I think its much simpler.
Thanks for the support!

[whoenig]

Hi Che,

I am glad that it worked for you so far!

I think your second code example is exactly what I was looking for (and I used that before and it gives reasonable performance since you get the data at fairly high frequency). The only problem is that you need to use a global variable. For python, there is good example code here: http://stackoverflow.com/questions/4233 ... eated-them

This is not as intuitive as global variables in C/C++. If you want to avoid global variables, you can create a class and then use member variables. ROS supports the callback function to be a member function of a class as well. Some example code on how to use classes with ROS is here:
https://github.com/whoenig/crazyflie_ro ... troller.py

Hope that helps!

Wolfgang

[Che.]

Hi Wolfgang,

The only problem is that you need to use a global variable.

I got everthing working so far, thanks again!
The link got me to prefix the keyword 'global' when changing my defined variables in the callback. However it is important to ensure that prefixing only when changing the variables, not when reading them (i.e. in my second callback function where I send those variables to the transform).

To save bandwidth, you might need to disable the IMU data collection in the ROS driver (required recompiling currently).

Could you elaborate that a bit further? I found the c++ of crazyflie_server and crazyflie_add, I assume I need to change crazyflie_server.cpp.

Does it suffice to delete the logging of sensor_msgs::MagneticField msg in the function Log2Data? Will running cmake be enough for compiling the cpp file?

Bandwidth might be a problem in my last part of development (can't test the whole system at the moment because I needed to change my sensor unfortunately at the last stage, and it has not arrived yet).

Otherwise, the simulation actually runs pretty smooth

Che.

[whoenig]

Hi Che,

Sounds great! To disable the unnecessary logging, comment/delete the following lines: https://github.com/whoenig/crazyflie_ro ... #L231-L255.
In your ROS workspace, run

Code: Select all

catkin_make

to recompile the code. I plan to give some more fine-grained control over which topics are enabled/disabled in the future so that recompiling becomes unnecessary.

Best,

Wolfgang

[Che.]

Hi Wolfgang,

controlling the topics from outside would be a nice feature. But the library is awesome already, thanks for that!

I'll keep this thread alive, but for now you have answered all my questions.

Greets,
Che.