whoenig / crazyflie_ros

ROS Driver for Bitcraze Crazyflie
MIT License
192 stars 205 forks source link

Dynamic waypoint tracking #23

Closed araujokth closed 8 years ago

araujokth commented 8 years ago

Hi,

I was trying to look at the demo "multi_waypoing_vicon" to try to understand how you could make the crazyflie track points in a sequential manner, but I could not understand how this is actually performed.

In my application, I would like to have the waypoints being defined by another application (say go to point A, then change to point B) and was wondering how to modify your code to allow this. In the hover application, the point is fixed and set inside the pose node, which runs publish_pose.py, where it is running continuously at the rate speed right?

Where would you suggest to be the best place to add a code that reads from a socket (to get new position waypoints) and sends those values to the controller? Also, if I wanted to use a specific yaw angle, how could this be done, since from what I understood in the current hover example that is not performed?

Since I do not have much experience with python, I would prefer to do it in c++, but I would go for python if that would be easier.

Thanks a lot for the help again and sorry if I have missed something in your examples that provide answers to my questions!

whoenig commented 8 years ago

Hi,

Does your application need feedback if the point was actually reached or will you give it "enough" time before you change to the next case? If you leave enough time between waypoints, you can simply replace publish_pose.py by something which reads the current waypoint from a socket and publishes it as PoseStamped in ROS. If you need the feedback it is more complicated, since you would need to check if the CF already reached the current point and send this (binary) information back to your external application via your socket.

Yaw is naturally supported, just set the rotation portion of PoseStamped accordingly. The teleop_vicon demo should have support for yaw as well (here, you change the goal point using your joystick, but the controller will make sure that the CF stays at the goal).

There is no strong reason for using python - the scripts just tend to be a bit shorter. For what you want to do, you can use C++ as well. Sockets might be more complicated though (you can look into boost asio) -- I believe python has native support for sockets.

Best,

Wolfgang

araujokth commented 8 years ago

Hi Wolfgang,

Thanks so much for the quick reply and tips. So actually what I needed is the teleop_vicon, but having the point set by the joystick to be set by my application instead. Thanks!

I will then try the socket with python inside the publish_pose.py. I was trying to edit that file in the afternoon to add a simple printf (tried also to use the log function you use inside controller.py there) to see how often it is being called and just get some output data but for some reason I always get an error when running the roslaunch. Do you know why that would be the case?

Thanks so much again!

whoenig commented 8 years ago

Python uses print() not printf. Often, ros-launch is not great about error-reporting. However, you can remove that node from the launch file, and run publish_pose.py directly from a second command prompt. This should give you some more details on what went wrong.

araujokth commented 8 years ago

Right, I meant print()! Ok, I will try that again tomorrow!

araujokth commented 8 years ago

One thing I forgot to ask. The rate parameter is in ms or?

araujokth commented 8 years ago

I think I was probably doing something wrong with the print command yesterday since it works now. I am starting to edit the file to add the communication with the socket.

I had a question regarding the waypoint tracking. How would it be possible to get the positions from other objects that are being captured by the mocap system into the demo application? the point was that I would like to have a "dynamic" landing pad where the CF should be landing at, instead of a fixed point, and since I have markers in the landing pad, I can set this position automatically. As for now, only the position of one or two CF is being used in the demo examples, but no other positions.

Thanks again!

araujokth commented 8 years ago

Hi Wolfgang,

I was having a look at your PID controllers and noticed that for the Z axis the gains are much higher than for the X and Y axis. Did you do this for any special reason when tuning the controller? I noticed that there is quite some overshoot in Z, while the X and Y tracking is smoother, so I was thinking of trying to modify the parameters a bit. In case you have tried some new controllers recently it would be great to know!

Just one additional question, after the inputs in each direction x, y, z and yaw are defined (msg.linear.x, etc), the roll, pitch, yaw and thrust will be defined in the file crazyflie_server.cpp in lines 164 to 167 right?

Thanks so much for the help again!

whoenig commented 8 years ago
araujokth commented 8 years ago

Thanks again Wolfgang!

I did not know that you had started to implement Mellinger's controller. Was going to start implementing it tomorrow morning. I was trying to figure out today if the coordinates/rotation that he uses would be ok to use with your current controller. I thought of using his trajectory tracking controller for that, generating a trajectory to the landing pad from a position like 30cm above the landing pad and x and y near the x and y of the landing pad center.

araujokth commented 8 years ago

I will try your implementation tomorrow and let you know of the results. I tried using your controller today and track first a point right above the target and then descend to the landing pad but it always ends up coming down too hard, so I think with trajectory (and potentially a velocity tracking part) will allow me to get a smooth and precise landing

I will keep you posted!

araujokth commented 8 years ago

Hi again Wolfgang,

When running the teleop_vicon in the Rviz, I am not getting any information regarding the battery of the crazyflie, neither the IMU data. I was wondering if this should be expected or there is some issue with my application? I was also thinking of trying to display other data there like the outputs from my controller and also perhaps the points of my computed trajectory which I want the CF to follow. Do you have any pointers on where I could find information on how to import such information into Rviz?

Thanks again!

whoenig commented 8 years ago

For your first issue, teleop_vicon has CF logging disabled for performance optimization. You can enable it again, by changing line https://github.com/whoenig/crazyflie_ros/blob/master/crazyflie_demo/launch/teleop_vicon.launch#L17.

RViz can show any topic which has a supported message type (and any topic if you are willing to write a plugin for a custom message type.) Furthermore, it supports markers (basically 3D objects you can put in 3D space) for further visualizations. Here, the same applies (i.e. the markers are just messages).

araujokth commented 8 years ago

Thanks for the help again! Is the performance loss due to the logging and display in Rviz or just because of the logging? In my application I really needed to keep track of the battery levels of the CFs, so I would need to have at least the battery being logged. If you have any tips regarding let me know.

araujokth commented 8 years ago

Hi Wolfgang,

just to let you know that I managed to read the battery in Rviz. (sorry for opening and closing the issue but pressed the wrong button).

binxxx commented 7 years ago

Hi guys,

Are you still working on the improvedControllerCpp branch right now? Recently I am trying to apply feed forward term in the PID controller to get better performance in trajectory tracking problem while the perform seems even worse. So I start to thinking about the methods in Mellinger's dissertation. Then I found that you were doing this around a year ago. So how is it going? Have you finished it yet?

When I try to git clone the improvedControllerCpp branch, then catkin_make this package. There is always a fatal error which is very weired for me. The error is as follows:

/home/binx/catkin_crazyflie/src/crazyflie_ros/crazyflie_controller/src/controller.cpp:1:21: fatal error: ros/ros.h: No such file or directory

Do you have any idea about that? Any suggestion would be appreciated.

Updated: The compile error is fixed. I guess when you working on this improvedControllerCpp, you were using previous ROS version. After I have added include_directories(${catkin_INCLUDE_DIRS}) after catkin_package(), I can compile with no errors right now. Hope this can work.

Updated: I've tested this controller but unfortunately it just does not work properly. Every time I send takeoff command, the CF would start to take off then when it goes up for around 3-5 cm, it would just shut down with no errors at all. I really don't know why is that. Please help.

Updated: The issue seems come from automatic case, since when linear.z increases over 50000, it would drops to around 100. So I guess it is because of automatic case.

Updated: https://github.com/whoenig/crazyflie_ros/blob/improvedControllerCpp/crazyflie_controller/src/controller.cpp#L212. Could you explain this code? How to get the thrust? I guess the error comes from here.

Updated: https://github.com/whoenig/crazyflie_ros/blob/improvedControllerCpp/crazyflie_controller/src/controller.cpp#L155. For this one line code, how do we confirm that it is the Z-axis of body frame? Sine I am using OptiTrack system, so I have converted the coordinate to align with ROS system. And it could work when I use the original controller in master branch. I think it might be the issue of how to generate the current Z-axis of body frame.

araujokth commented 7 years ago

Hi, I tried using it but since I managed to get quite a good behavior with the PID I did not look further into this other controller. My feeling is that if you do not want to do anything super fancy with your flying, it may be better to spend a bit more time adjusting the PID than to go for this other controller

binxxx commented 7 years ago

Hi

Can you use this improveControllerCpp when you once tried that?

I have also tuned the PID parameters for quite some time. The hover quality is quite good but the overshoot is too large. And I also want to achieve trajectory tracking, I found that the tracking quality is really bad. Since it is a little different with the so called way point tracking. Could you share your PIDs parameters with me? I can compare mine with yours. And do you ever tried trajectory tracking, like tracking a circle? The problem for me is that the CF just cannot catch up with the new desired point and which makes the actual trajectory not quite like a circle. I need't stick to the new controllers, but I just want to make the tracking quality better. Do you have any suggestions?

Thanks.

mikehamer commented 7 years ago

Hi Guys,

Having just been brought into this thread, I'm not sure exactly what improveControllerCpp is. I have been using my quaternion-based controller very successfully for aggressive trajectory flight. I find this controller to be very intuitive to tune in comparison to other controllers that I've seen around, and it seems to be more robust to inter-quad variation than other controllers (eg. parameters that work well for one quad, tend to work well for other quads as well, even if slightly more beaten up).

My controller allows feedforward terms to be sent down to the body-rate level, and I achieve fairly good performance on pre-planned trajectories when using this. Certainly onboard localisation and control are very important for very aggressive flight, but if you're planning on flying slowly I'm sure things can be done offboard.

I haven't used improveControllerCpp, so cannot really compare. But perhaps this response answers your questions?

Mike

whoenig commented 7 years ago

improveControllerCpp is not really used anymore. However, Mellingers controller is fully implemented and used as part of the Crazyswarm project. In contrast to the version here, the controller runs fully onboard and thus is much more robust. You can find the firmware implementation https://github.com/USC-ACTLab/crazyflie-firmware/blob/crazyswarm_master/src/modules/src/position_controller_mellinger.c.

I wont comment on the various errors you see in this branch as this code is simply outdated and not supported.

binxxx commented 7 years ago

I'm really sorry for bringing you in this thread by mistake. @mikehamer

But you did give me new solution to do something with my work. I haven't got the time to use your controller yet. But I've just read a little bit about the modified version of crazyflie_ros, there seems no change in controller.cpp. I thought if you want to add feedforward term, that would be a good place to do that. I would be very appreciated if you could explain more about your approach to modify both the firmware and ROS package.

Thanks.

binxxx commented 7 years ago

Thanks for your information. @whoenig I would give up on that.

I'm curious about if the firmware you provided is still compatible with your ROS package. I'm going to do research with loader carrier while the first thing is to make the nominal trajectory precise and stable. But to make this happen really takes me quite a lot time. The outcome is still not quite good. Thanks for your help.

binxxx commented 7 years ago

Hi @whoenig

Sorry to bother you again. I have git clone the crazyswarm_master branch from your lab github page. But when I want to catkin_make, these errors came out.

Base path: /home/binx/catkin_crazyflie
Source space: /home/binx/catkin_crazyflie/src
Build space: /home/binx/catkin_crazyflie/build
Devel space: /home/binx/catkin_crazyflie/devel
Install space: /home/binx/catkin_crazyflie/install
####
#### Running command: "make cmake_check_build_system" in "/home/binx/catkin_crazyflie/build"
####
####
#### Running command: "make -j4 -l4" in "/home/binx/catkin_crazyflie/build"
####
[  0%] Building CXX object crazyflie_ros/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyflie.cpp.o
[  0%] Building CXX object crazyflie_ros/externalDependencies/libmotioncapture/CMakeFiles/libmotioncapture.dir/src/vicon.cpp.o
[  1%] Built target libobjecttracker
[  1%] Built target _optitrack_generate_messages_check_deps_RigidBody
[  1%] Built target std_msgs_generate_messages_eus
In file included from /home/binx/catkin_crazyflie/src/crazyflie_ros/crazyflie_cpp/include/crazyflie_cpp/Crazyflie.h:7:0,
                 from /home/binx/catkin_crazyflie/src/crazyflie_ros/crazyflie_cpp/src/Crazyflie.cpp:4:
/home/binx/catkin_crazyflie/src/crazyflie_ros/crazyflie_cpp/include/crazyflie_cpp/crtp.h:2:23: fatal error: packetdef.h: No such file or directory
compilation terminated.
crazyflie_ros/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/build.make:134: recipe for target 'crazyflie_ros/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyflie.cpp.o' failed
make[2]: *** [crazyflie_ros/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyflie.cpp.o] Error 1
CMakeFiles/Makefile2:378: recipe for target 'crazyflie_ros/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/all' failed
make[1]: *** [crazyflie_ros/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/all] Error 2
make[1]: *** Waiting for unfinished jobs....
[  2%] Building CXX object crazyflie_ros/externalDependencies/libmotioncapture/CMakeFiles/libmotioncapture.dir/src/optitrack.cpp.o
[  2%] Built target geometry_msgs_generate_messages_eus
[  2%] Built target _optitrack_generate_messages_check_deps_RigidBodyArray
/home/binx/catkin_crazyflie/src/crazyflie_ros/externalDependencies/libmotioncapture/src/vicon.cpp:4:30: fatal error: vicon_sdk/Client.h: No such file or directory
compilation terminated.
crazyflie_ros/externalDependencies/libmotioncapture/CMakeFiles/libmotioncapture.dir/build.make:86: recipe for target 'crazyflie_ros/externalDependencies/libmotioncapture/CMakeFiles/libmotioncapture.dir/src/vicon.cpp.o' failed
make[2]: *** [crazyflie_ros/externalDependencies/libmotioncapture/CMakeFiles/libmotioncapture.dir/src/vicon.cpp.o] Error 1
make[2]: *** Waiting for unfinished jobs....
/home/binx/catkin_crazyflie/src/crazyflie_ros/externalDependencies/libmotioncapture/src/optitrack.cpp:4:38: fatal error: NatNetLinux/NatNetClient.h: No such file or directory
compilation terminated.
crazyflie_ros/externalDependencies/libmotioncapture/CMakeFiles/libmotioncapture.dir/build.make:110: recipe for target 'crazyflie_ros/externalDependencies/libmotioncapture/CMakeFiles/libmotioncapture.dir/src/optitrack.cpp.o' failed
make[2]: *** [crazyflie_ros/externalDependencies/libmotioncapture/CMakeFiles/libmotioncapture.dir/src/optitrack.cpp.o] Error 1
CMakeFiles/Makefile2:1049: recipe for target 'crazyflie_ros/externalDependencies/libmotioncapture/CMakeFiles/libmotioncapture.dir/all' failed
make[1]: *** [crazyflie_ros/externalDependencies/libmotioncapture/CMakeFiles/libmotioncapture.dir/all] Error 2
Makefile:138: recipe for target 'all' failed
make: *** [all] Error 2
Invoking "make -j4 -l4" failed

It shows that some cpp header files, packetdef.h vicon_sdk/Client.h NatNetLinux/NatNetClient.h cannot be found. I've searched for a while and still cannot find the exact same header files. How can I solve this? Thanks in advance.

whoenig commented 7 years ago

Please follow the instructions at https://github.com/USC-ACTLab/crazyswarm. There is a build script, which will try to download all dependencies first. Also, please open a new issue (in the respective repositories) if you have any trouble.

binxxx commented 7 years ago

Thank you very much for your help. Sure, I will open new issue if I encounter some new problems.

RozDavid commented 6 years ago

Hi @whoenig, Thank you very much for all your work. I would like to have your advice, what do you suggest, how should I implement a teleoperation with the new, Mellinger control method.

So right now I managed to use and test the teleoperation with vrpn, using goal_position updating, but as it uses the old PID control, it is very unstable especially for the take_off commands. I also compiled, studied and tested your crazyswarm project, but in that case it was harder to use the teleoperation. I tried publishing a goal_pose here too, and updating the goTo function perioically that worked good for waypoint following, but didn't make a smooth flight in the velocity. It started and stopped for every iteration thus making the flight uncontrolled.

So I changed back to the latest version of crazyflie_ros, and would like to implement the teleoperation in this project. What do you suggest, how could I make it the best, without hardcore developing?

I tested with the Position.py idea, with cf1/cmd_position commands, but that didn't even moved the rotors. How shall this node work? Can I use it for teleoperation, and if yes, shall I compute the cmd_vel values myself? FYI I didn't managed to fly with the Hover.py either. For these scripts should I make the firmware with controller=mellinger and kalman parameters?

Sorry for the far too many questions, but hope you will have the time to answer them.

Best wishes

whoenig commented 6 years ago

Hi,

The latest firmware(s) come with the kalman filter and mellinger controllers included and you can switch to them at runtime (thus, there is no special build needed; see https://github.com/USC-ACTLab/crazyswarm/blob/master/ros_ws/src/crazyswarm/launch/hover_swarm.launch#L24-L26 for the parameter names). The scripts in crazyflie_demo do not reflect that firmware update, yet.

For full control, I suggest sending cmd_full_state commands. cmd_position should technically work fine, although we don't really have tests for that yet.

RozDavid commented 6 years ago

Hi @whoenig,

Thank you very much for your help, but I am still a bit confused right now. I tried publishing the cmd_full_state values for a crazyflie updated with the crazyswarm firmwares under the topic of /cf1/cmd_full_state for example, but that made no result at all. I wasn't sure, the cf subscribed to that topic. After taking of with high level commands, it would not respond for any of the cmd_position/vel/full_state commands.

After I tried using this full state movement in the crazyflie_ros, after all the crazyflie responded, but sadly didn't reached the state that is should. There is a sample video showing the response for the exacute_trajectory.py script with takeoff.csv file. For your information when trying to use the test_high_level scri with the lates crazyflie_ros and the 2018.01 firmware the output was:

[ERROR] [1537440513.414927950]: Could not find param commander/enHighLevel
[ERROR] [1537440513.416744030]: client wants service /crazyflie/takeoff to have md5sum b665b6c83a196e4774268cc26329b159, but it has d41d8cd98f00b204e9800998ecf8427e. Dropping connection.

So my questions would be:

And again sorry for the question, but your answers are really highly appreciated.

whoenig commented 6 years ago
  1. cmd_full_state is not implemented in the Crazyswarm, yet. You'll need to use crazyflie_ros instead.
  2. The crazyflie_controller package is obsolete/not recommended to use (it essentially is still there as an example on how one could write an external controller). The link to the launch file was only to tell you the names of the parameters: "stabilizer/estimator" (set to 2) and "stabilizer/controller" (set to 2). There is an example here on how to change parameters within crazyflie_ros: https://github.com/whoenig/crazyflie_ros/blob/master/crazyflie_demo/scripts/test_high_level.py#L13. Also note that you'll need the latest official firmware from github - I think 2018.01 does not have all the changes needed to switch estimator/controller dynamically.
  3. No. Mellinger only works with cmd_* commands, and high-level command mode (that runs on-board the firmware).
  4. Which controller are you using? There should be no oscillations if you use the mellinger controller.