[Video] Alternative Kinect Control (using depth images)
-
- Member
- Posts: 45
- Joined: Mon Jan 13, 2014 4:12 am
Re: [Video] Alternative Kinect Control (using depth images)
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?
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?
-
- Expert
- Posts: 162
- Joined: Thu Jun 06, 2013 9:56 pm
- Location: Munich
Re: [Video] Alternative Kinect Control (using depth images)
Yup! You will need to fake a joy message and send it at 100hz.Veerachart wrote: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?
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.
-
- Member
- Posts: 45
- Joined: Mon Jan 13, 2014 4:12 am
Re: [Video] Alternative Kinect Control (using depth images)
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?omwdunkley wrote: 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.
I'll try it tomorrow or so. Hope it works somehow!
-
- Expert
- Posts: 162
- Joined: Thu Jun 06, 2013 9:56 pm
- Location: Munich
Re: [Video] Alternative Kinect Control (using depth images)
Check this out, it will help youYep, 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?
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)
Re: [Video] Alternative Kinect Control (using depth images)
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.omwdunkley wrote: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).fetrit wrote: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
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
Re: [Video] Alternative Kinect Control (using depth images)
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:
Thought I'd make a list of how our procedure is to start the tracking:
Patrik
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:
So what we did was under the log tab in MainWindow (driver.py):GUI
log | pm | at minimum roll, pitch and yaw checked.
log | pm checked
log | pm | set hz to 100
- - 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.
Thought I'd make a list of how our procedure is to start the tracking:
- - Run roscore
- Run the transforms:- Launch the kinect drivers: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
- Connect the PS3-controller.Code: Select all
roslaunch freenect_launch freenect.launch
- Launch the PID controller- In the Reconfigure window under "/camera/driver" check "depth registration"Code: Select all
roslaunch crazyflieROS pid.launch js:=0
- 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
- In rviz add TF under add-> "by display type"-tab ->TFCode: Select all
rosrun rviz rviz
- Also add a camera under add-> "by topic"-tab ->/camera->/depth_registered->/image_raw->Camera(raw)
- Run the MainWindow:- Under the Log tab:Code: Select all
rosrun crazyflieROS 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.
- - Disable Thrust, unchecked
- Disable Hover Mode, checked
- X-mode, unchecked
- 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. - - Control, checked
Patrik
Last edited by Patrik on Wed May 21, 2014 1:44 pm, edited 1 time in total.
Re: [Video] Alternative Kinect Control (using depth images)
Ok, so we get the following error message:
Could you help us fix it Oliver?
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
Re: [Video] Alternative Kinect Control (using depth images)
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*
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*
-
- Expert
- Posts: 162
- Joined: Thu Jun 06, 2013 9:56 pm
- Location: Munich
Re: [Video] Alternative Kinect Control (using depth images)
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.
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.
-
- Member
- Posts: 45
- Joined: Mon Jan 13, 2014 4:12 am
Re: [Video] Alternative Kinect Control (using depth images)
I just tried with writing a simple (maybe silly ) code to send the fake joy signal. It looks something like this:
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 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.
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()
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"):
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.
Last edited by Veerachart on Thu May 22, 2014 7:39 am, edited 2 times in total.