Bitcraze Forums

Hi guys;
I downloaded crazyflie_ros document (it's C++ CODED) and I'm searching for the function that gets my packet from API/SLAM data, so I opened crazyflie_server.cpp its link here:
https://github.com/whoenig/crazyflie_ro ... server.cpp
but I'm not finding the function on that server script that's getting data from sensors or actually from flowdeck sensor in order to get hieght measurement or x measurement or y measurement .. any help please to figure out which function is getting data about the coordination (x , y , z) from sensors or data from flowdeck sensor ?! thanks alot !!

Any help guys?!

The ROS driver is setting up a log block for the Crazyflie position there: https://github.com/whoenig/crazyflie_ro ... #L517-L523 with a callback that publishes the pose as a ROS TF message: https://github.com/whoenig/crazyflie_ro ... r.cpp#L656. You can get the full pose, including position, of the Crazyflie in ROS using the TF ros subsystem.

Though for SLAM you will also want ranges from the multiranger. To get that you can setup custom log block in your launchfile that are published as ROS topics by the Crazyflie ROS driver.

Hi , I understand your effort.
Your last note about slam I didn't get you well .. may you elaborate more?
what I want to do that by SLAM'S data for instance the height z that's calculated by SLAM I want to send that data to the server of crazyflie that's set up on the crazyflie's chip ..so how I can do that? in other words, I don't want to get height measurement by flow deck and I want to get by SLAM'S data .. how do I do that thing?!

By SLAM I understand "Simultaneous Location And Mapping". Since you are looking in the ROS driver, I am assuming you are interested in implementing SLAM on the ROS side (for example using the existing ROS navigation stack, http://wiki.ros.org/navigation). In that case you will need both the Crazyflie full pose and the ranging measurement of the 4 horizontal ranging sensors from the multiranger.

There is no SLAM implementation in Crazyflie. The Crazyflie has a state estimator that estimates the full pose. The full pose estimated by the Crazyflie is logged and made available by the ROS driver.

Hi ! much appreciated your effort to answer me.

so in my problem, if I need the data that's the output of the slam which I can use it as a localization of where my crazyflie is standing instead of using measurement of flowdeck ... is much complicated? but if I have data from slam as x , y , z ..it looks really easy to send them to the server of crazyflie chip .. why not? or what is the problem with?

How can I overcome on that problem?
*I will detail more about the problem, the problem is:
it's obvious that we get hovering with crazyflie by using flowdeck sensor's data, so what I want instead of those flowdeck sensor data to replace them with slam over ROS DATA ..

Ho, I think I now undestand your question. Please tell me if this is not correct: you are acquiring position measurement of the Crazyflie outside the Crazyflie and you want to push these position measurement so that it can be used in the Crazyflie.

This is possible and it is what is done when flying in a MoCap. The code handling that in the ROS driver is there: https://github.com/whoenig/crazyflie_ro ... r.cpp#L357. Position measurement are apparently sent to the Crazyflie when the TF 'm_tf_prefix + "/external_position"' changes (https://github.com/whoenig/crazyflie_ro ... r.cpp#L377) so all you should have to do is to push your position measurement to it.

Note that this is going to send an external position packet and that this is pushing the position in the EKF with a standard deviation of 0.01m by default (https://github.com/bitcraze/crazyflie-f ... ice.c#L140).

I didn't understand you exactly, could you explain please more?

on the screen on my terminal on linux I see the data x , y , z that slam is measuring .. so what should I do in order to send them to my crazyflie ? I need to send them to be able to hover my crazyflie on specific height by using slam's data and not FLOWDECK

much appreciate if you could help me

The Crazyflie ROS driver is using the ROS TF package (http://wiki.ros.org/tf) to send position measurement to the Crazyflie. So from your SLAM code you need to publish a TF transform containing your measurements, and the Crazyflie ROS driver will then send these measurement to the Crazyflie.

I have been browsing the Crazyflie ROS example to find an example and there seems to be a way to just publish a position topic to the Crazyflie as well though. You can look at this example, it is pushing position to the Crazyflie from a ROS node: https://github.com/whoenig/crazyflie_ro ... on_vrpn.py

Hi arnaud ;
I'm still can't get hovering by slam data instead of flowdeck, I have the output of slam and I did as what you said, once I want to run my python script for hovering, my crazyflie just running the motors and not flying to specific height. any help please? I guess my crazyflie isn't getting a proper data from slam .. but it's weird because Im sending to it TF packet as what you said before ..

my node name that's publishing TF message named : publish_external_position_RS_OBC

here's my script: (publish_external_position_RS_OBC.py)
#!/usr/bin/env python
import rospy
import tf
from nav_msgs.msg import Odometry
from geometry_msgs.msg import PointStamped, TransformStamped, PoseStamped, PoseWithCovarianceStamped #PoseStamped added to support vrpn_client
from crazyflie_driver.srv import UpdateParams
import time
from std_msgs.msg import String

X0 = 0
Y0 = 0
Z0 = 0

out = False


firstTransform = True


def onNewTransform(pose):
global msg
global pub
global firstTransform

global X0
global Y0
global Z0

rospy.loginfo("sec\n\n")
if firstTransform:
rospy.set_param("kalman/initialX", 0) #pose.pose.position.x) # Change pose according to your topic
rospy.set_param("kalman/initialY", 0) #pose.pose.position.y)
rospy.set_param("kalman/initialZ", 0) #pose.pose.position.z)
update_params(["kalman/initialX", "kalman/initialY", "kalman/initialZ"])
rospy.set_param("kalman/resetEstimation", 1)
update_params(["kalman/resetEstimation"])
X0 = pose.pose.pose.position.x;
Y0 = pose.pose.pose.position.y;
Z0 = pose.pose.pose.position.z;
firstTransform = False

else:
msg.header.frame_id = pose.header.frame_id # Change pose according to your topic
msg.header.stamp = pose.header.stamp
msg.header.seq += 1


X2 = pose.pose.pose.position.x;
Y2 = pose.pose.pose.position.y;
Z2 = pose.pose.pose.position.z;

msg.point.x = (X2 - X0)*1
msg.point.y = (Y2 - Y0)*1
msg.point.z = (Z2 - Z0)*1

#rospy.loginfo("x=" + str(pose.pose.pose.position.x)[:6] + "... y=" + str(pose.pose.pose.position.y)[:6] + "... z=" + str(pose.pose.pose.position.z)[:6])
#rospy.loginfo("x0=" + str(X0)[:6] + "... y0=" + str(Y0)[:6] + "... z0=" + str(Z0)[:6])
rospy.loginfo("x_sent=" + str(msg.point.x)[:6] + "... y_sent=" + str(msg.point.y)[:6] + "... z_sent=" + str(msg.point.z)[:6])
#rospy.loginfo(str(pose.pose.pose.position.x) + " " + str(pose.pose.pose.position.y) + " " + str(pose.pose.pose.position.z))
#rospy.loginfo(str(X0) + " " + str(Y0) + " " + str(Z0))
#rospy.loginfo(str(msg.point.x) + " " + str(msg.point.y) + " " + str(msg.point.z))
pub.publish(msg)



def callback(data):
rospy.loginfo("enter ..HON..")
global out
if data.data == "true":
out = True
return


if __name__ == '__main__':
#rospy.init_node('publish_external_position_RS_OBC', anonymous=True)
#topic = rospy.get_param("~topic", "/crazyflie7/vrpn_client_node/crazyflie7/pose")
rospy.init_node('publish_external_position_RS_OBC', anonymous=True)
topic = rospy.get_param("~topic", "/crazyflie1/vrpn_client_node/crazyflie1/pose")
rospy.wait_for_service('update_params')
rospy.loginfo("found update_params service")
update_params = rospy.ServiceProxy('update_params', UpdateParams)
firstTransform = True
msg = PointStamped()
msg.header.seq = 0
msg.header.stamp = rospy.Time.now()

#pub = rospy.Publisher("/external_position", PointStamped, queue_size=1)
pub = rospy.Publisher("external_position", PointStamped, queue_size=1)
#rospy.loginfo("enter ..TEST.")
#rospy.Subscriber("/chatter", String, callback)
#while (out == False):
# pass
#time.sleep(2)
#rospy.loginfo("tryrrR")

#rospy.Subscriber("/orb_slam2_pose", PoseWithCovarianceStamped, onNewTransform) # Change this according to your topic
rospy.Subscriber(topic, PoseStamped, onNewTransform)
rospy.spin()

--------------------------------------------------------------------------------------------------------------------------------

and I run this script for telling to crazyflie to hover in specific height: (named test_high_level_RS_OBC22.py)
#!/usr/bin/env python

import rospy
import crazyflie
import time
import uav_trajectory

if __name__ == '__main__':
rospy.init_node('test_high_level')

cf = crazyflie.Crazyflie("crazyflie3", "crazyflie3")

cf.setParam("commander/enHighLevel", 1)

cf.takeoff(targetHeight = 0.3, duration = 5.0)
time.sleep(80.0)

cf.land(targetHeight = 0.0, duration = 2.0)

#traj1 = uav_trajectory.Trajectory()
#traj1.loadcsv("takeoff.csv")

#traj2 = uav_trajectory.Trajectory()
#traj2.loadcsv("figure8.csv")

#print(traj1.duration)

#cf.uploadTrajectory(0, 0, traj1)
#cf.uploadTrajectory(1, len(traj1.polynomials), traj2)

#cf.startTrajectory(0, timescale=1.0)
#time.sleep(traj1.duration * 2.0)

#cf.startTrajectory(1, timescale=2.0)
#time.sleep(traj2.duration * 2.0)

#cf.startTrajectory(0, timescale=1.0, reverse=True)
#time.sleep(traj1.duration * 1.0)

cf.stop()





-----------------------------------------------------

since I run the second script (the second script that I posted here) then my crazyflie is just moving its motors and not hovering ! any help?! I guess that Im forgetting something but couldn't find it !


thanks in advance