[Video] Alternative Kinect Control (using depth images)

[Veerachart]

Well, a quick look through Patrik's graph suggests me that the node CrazyFlieJoystickDriver is not publishing any transformations, and after diving a little into the file joy_driver_pid.py, I found that those transformations occur in function getErrorToGoal, which is called here inside the function new_joydata, which is the callback function for the subscriber sub_joy on topic /joy.

So, it seems like if I publish correct messages to topic /joy, at the correct frequency, it will work (right?)

Then, how can I know what is the "correct" form for /joy?

[omwdunkley]

Well, a quick look through Patrik's graph suggests me that the node CrazyFlieJoystickDriver is not publishing any transformations, and after diving a little into the file joy_driver_pid.py, I found that those transformations occur in function getErrorToGoal, which is called here inside the function new_joydata, which is the callback function for the subscriber sub_joy on topic /joy.

So, it seems like if I publish correct messages to topic /joy, at the correct frequency, it will work (right?)

Then, how can I know what is the "correct" form for /joy?

Yup! You will need to fake a joy message and send it at 100hz.
The joy message definition is here.
You can probably check the code to see which axes needs which information. I defined "enums" for Buttons.XYZ and Axes.XYZ https://github.com/omwdunkley/crazyflie ... id.py#L179. They map the axes/button number to the label. Eg Axes.L1 maps to number 10, so you will need to have that in your message that you send (and in the case of L1, set it to -1.0, as the code checks if it is < -0.75).

In theory your approach should work. In practise it might be a little hard to debug if something goes wrong.

[Veerachart]

Yup! You will need to fake a joy message and send it at 100hz.
The joy message definition is here.
You can probably check the code to see which axes needs which information. I defined "enums" for Buttons.XYZ and Axes.XYZ https://github.com/omwdunkley/crazyflie ... id.py#L179. They map the axes/button number to the label. Eg Axes.L1 maps to number 10, so you will need to have that in your message that you send (and in the case of L1, set it to -1.0, as the code checks if it is < -0.75).

In theory your approach should work. In practise it might be a little hard to debug if something goes wrong.

Yep, I saw that definition. Just to check my understanding, buttons and axes are a fix-length array with every member equals to 0 except the pressed button, aren't they? And do I have to bother with the header, or just leave it like that and assign only values for buttons and axes?

I'll try it tomorrow or so. Hope it works somehow!

[omwdunkley]

Yep, I saw that definition. Just to check my understanding, buttons and axes are a fix-length array with every member equals to 0 except the pressed button, aren't they? And do I have to bother with the header, or just leave it like that and assign only values for buttons and axes?

Check this out, it will help you

As for the header, I only use the sequence counter to limit the frequency that I update the goal with. You can safely just ignore the header (it will all default to 0).

Good luck)

[fetrit]

Thanks for the reply,

Is there a way to get the acceleration in cartesian coordinates in your code? I've tried to look at your code and the closest I found was the "self.acc" but I'm not sure if this is the right variable. I need it for the kalman filter

I guess you could use the roll, pitch, yaw information to rotate the acceleration vector. I once did this on the flie, cant remember where I put the code though. Note that yaw drifts unless you compensate (external tracking, magnetometer, etc).

I have had a look in the different files in the joyManager branch and I am a bit confused where to find the roll, pitch, yaw and thrust.

Do you think it possible to get the roll, pitch, yaw and thrust from the file "cmd.msg" and import the values to the trackManager.py where I will put the kalmanfilter function? Or is this the wrong values to get?

I've also found something in the file commander.py in the crazyflieROS / src / crazyflieROS / cflib / crazyflie / commander.py
I was then think of writing a function in the class "Commander()" that would return the roll, pitch, yaw and thrust. The kalmanfilter function in the trackManager.py would call this function in the Commander class getting these values.

Again, I don't know if this is the right values to get and if it is possible

[Patrik]

Hi Oliver!

From your feedback i guess it might be something with the transforms. I'll make a cube as well

The problem we solved to start the tracking was to understand what the following meant in the read me:

GUI

log | pm | at minimum roll, pitch and yaw checked.
log | pm checked
log | pm | set hz to 100

So what we did was under the log tab in MainWindow (driver.py):

• - For pm and stabilizer, set HZ desired (in the third column) to 100.
  - Check the box for pm.
  - Check the boxes pitch roll and yaw in stabilizer.

Hope this is all you should do in the log tab, otherwise let me know.

Thought I'd make a list of how our procedure is to start the tracking:

• - Run roscore
  - Run the transforms:

  Code: Select all

  rosrun tf static_transform_publisher -1.5 0 1 0 0 0 /world /camera_link 10
  rosrun tf static_transform_publisher 0 0 0 0 0 0 /cf0 /cf_gt 10
  

  - Launch the kinect drivers:

  Code: Select all

  roslaunch freenect_launch freenect.launch
  

  - Connect the PS3-controller.
  - Launch the PID controller

  Code: Select all

  roslaunch crazyflieROS pid.launch js:=0
  

  - In the Reconfigure window under "/camera/driver" check "depth registration"
  - Also in the Reconfigure window under /CrazyFlieJoystickDriver check/uncheck:
  • - Control, checked
    - LiveUpdate, unchecked (this lets you change goal live)
    - Set goal, click (this sets the goal when you click it)
    - xyControl, checked
    - yawControl, checked
    - thrustControl, checked
    - PIDPreset should be set to Passive
    - max_thrust should be set to 100
  - Run rviz:

  Code: Select all

  rosrun rviz rviz
  

  - In rviz add TF under add-> "by display type"-tab ->TF
  - Also add a camera under add-> "by topic"-tab ->/camera->/depth_registered->/image_raw->Camera(raw)
  - Run the MainWindow:

  Code: Select all

  rosrun crazyflieROS driver.py
  

  - Under the Log tab:
  • - For pm and stabilizer, set HZ desired (in the third column) to 100.
    - Check the box for pm.
    - Check the boxes pitch roll and yaw in stabilizer.
  - Under the Input tab check/uncheck the following:
  • - Disable Thrust, unchecked
    - Disable Hover Mode, checked
    - X-mode, unchecked
  - Under the Settings tab check yaw offset
  - From the read me:

  Align the crazyflie with the kinect (either crazyflie x (front) aligned with kinect z (optical axis), or crazyflie -y (right) aligned with kinect z. The press "set north". This is needed as the kienct cannot determine the yaw of the crazyflie, so we use the onboard attitude estimation for this. But it first needs to be aligned with the other frames.

Hope this is of use for someone

Patrik

[Patrik]

Ok, so we get the following error message:

Code: Select all

[ERROR] [WallTime: 1400679220.581447] bad callback: <bound method KinectTracker.incomingDepthData of <trackManager.KinectTracker object at 0x92f5bb4>>
Traceback (most recent call last):
  File "/opt/ros/hydro/lib/python2.7/dist-packages/rospy/topics.py", line 682, in _invoke_callback
    cb(msg)
  File "/home/bitcraze/projects/crazyflieROS/src/crazyflieROS/trackManager.py", line 568, in incomingDepthData
    flies = sorted([f.extend( (f[2]-self.goal[0])**2+(f[3]-self.goal[1])**2+(f[4]-self.goal[2])**2) for f in flies], key=itemgetter(7),reverse=False)
IndexError: list index out of range

Could you help us fix it Oliver?

[Patrik]

Soo...

When I listen to the /cfjoy topic it doesn't change the roll, pitch or yaw angle, it just sets the thrust to 32000 something and says hover mode true when we try to track and control a dummy flie (a battery on a string ). The previous error doesn't come up again..

Seems our project might be a failure *sob*

[omwdunkley]

Patrik,
About that error, not sure what it is, I wont have time to look into it until the end of the week or so. I plan to do some experiments with the whole setup next week. I can debug more then. Im sure its just stupid/small mistake somewhere. Sorry for the delay, Im under a lot of stress right now :/
Thanks for the write up thought!
Double check that the tracking and goal position that are displayed in rviz make sense, that the flie is on and that its publishing. You can turn of the thrust in the GUI so that even if the PID controller is sending thrust commands, the flie wont move. Good for debugging.

Ferit,
What exactly do you need, I am a little confused to what it is you are trying to do.
The flie has measured roll, pitch, yaw; and the GUI sends roll, pitch, yaw, thrust commands to the flie.
I assume you want to fuse the latest sensor data as measured by the flie with the tracker estimates.

Maybe a new node would be easiest for you. You could write a ros node that listens to the messages from the flie that it needs (acceleration, gyro, barometer, whatever). You could also add some code (I could possibly help with this next week) that would sent out all the potential flie detections (x,y,z, size, distance from background, pixel coordinates, pixel width, estimated object width) over a topic. Then your new node would listen to all of this and you could fuse away as you wish.

[Veerachart]

I just tried with writing a simple (maybe silly ) code to send the fake joy signal. It looks something like this:

Code: Select all

#!/usr/bin/env python

import rospy

from sensor_msgs.msg import Joy as JoyMSG

class FakeJoyNode:
    def __init__(self):
        #publisher for fake joy signal
        self.joy_pub = rospy.Publisher('joy', JoyMSG)
        self.joy_msg = JoyMSG()
        self.joy_msg.axes = [0,0,0,0,0,0,0,0,0,0,-1.0,0,0,0,0,0,0,0,0,0]
        self.joy_msg.buttons = [0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0]
        
    def run_node(self):
        self.joy_pub.publish(self.joy_msg)
        
if __name__ == '__main__':
    rospy.init_node('fakeJoyPublisher')
    
    node = FakeJoyNode()
    r = rospy.Rate(100)
    while not rospy.is_shutdown():
        node.run_node()
        r.sleep()

Well, both 1 in buttons and -1 in axes are needed, as in the code it checks if L1 is pressed and holded.
Then, I tried with the tracking. Now the frame /goal appeared and can be controlled by the GUI if the box LiveUpdate is checked. However, frames /cf0 and /cf_gt are not yet published (also /cf_gt2d - I got a warning of "Could not look up cf_gt2d -> goal").

Anyway, at least now the flie started, but at constant thrust of 32767 (I subscribed to /cfjoy and got this number.)

EDIT:
By digging further through the code, I found that the condition

Code: Select all

if isGroup(log, "stabilizer"):

in rosTools.py line 109 is not entered, causing the frame /cf0 not published. The cause is still a mystery.

EDIT 2:
Unchecking thrust in stabilizer from the log makes stabilizer available in log and now all frames appears !
Anyway, I also got similar problem as Patrik, that the flie just went off like crazy with thrust = 54000, even though I set the goal close to the current position of the flie.