Bitcraze Forums

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>

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

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'

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

Code: Select all

catkin_make