How does controller.cpp receive the postion information and how to generate the control input

[zhanghaijason]

Hi whoenig, I read the code again recently and still have several questions. For the hovering problem:

1. How could ROS know CF's position? I know it should be related to targetDrone,

`tf::StampedTransform transform; m_listener.lookupTransform(m_worldFrame, m_frame, ros::Time(0), transform);

            geometry_msgs::PoseStamped targetWorld;
            targetWorld.header.stamp = transform.stamp_;
            targetWorld.header.frame_id = m_worldFrame;
            targetWorld.pose = m_goal.pose;

            geometry_msgs::PoseStamped targetDrone;
            m_listener.transformPose(m_frame, targetWorld, targetDrone);

            tfScalar roll, pitch, yaw;
            tf::Matrix3x3(                                       //tf namespace, Matrix3x3, class
                tf::Quaternion(                                  //tf namespace, Quaternion, class
                    targetDrone.pose.orientation.x,
                    targetDrone.pose.orientation.y,
                    targetDrone.pose.orientation.z,
                    targetDrone.pose.orientation.w
                )).getRPY(roll, pitch, yaw);`

Which sentence actually gets the CF's position? How could it get the position data? In my understanding, the vicon/optitrack node should publish the position data to a topic A and the controller.cpp subscribes to topic A to get the position data. But I did not find such publisher in vicon's code, either the subscriber in controller.cpp 2. For the PID controller The input to these controllers are the desired hover position and the current feedback. Is it realistic to get the desired control input(pitch, roll, yaw rate, thrust) from the error one to one?(I mean control input roll is generated only from x position error, desired control input pitch is generated only from y position error... ).

Thanks

[whoenig]

1. vicon_bridge tracks the position relative to /world and publishes transforms. See https://github.com/whoenig/crazyflie_ros/blob/master/crazyflie_demo/launch/hover_vicon.launch#L4 for the frame used. The lookupTransform function gets the pose (position and orientation).
2. This is a very simple controller. There are much better non-linear controllers, one of which is implemented as part of the Crazyswarm project in a custom CF firmware.

[zhanghaijason]

Hi @whoenig,

1. I use the mocap_optitrack package to get the pose feedback. After rosrun tf view_frames , the pdf I got is frames.pdf. And roslaunch crazyflie_demo hover_optitrack.launch uri:=radio://0/100/2M/E7E7E7E701 frame:=crazyflie1 x:=0 y:=0 z:=0.5 rosrun tf view_frames, what I got is whole_frames.pdf

I can see the position of CF in RVIZ, and the coordinate seems correct, the movement of CF in RVIZ is correct. However, I cannot make it take off with ps3. What's more, the movement of CF in RVIZ always get stuck. Following are the ROS INFO: [ INFO] [1504534120.316177571]: Requesting parameters... Params: 117 [ INFO] [1504534120.983451667]: Ready... [ INFO] [1504534120.983486107]: Elapsed: 0.667328 s [INFO] [1504534121.067695]: found update_params service [INFO] [1504534121.068097]: waiting for emergency service [INFO] [1504534121.069096]: found emergency service [INFO] [1504534121.069332]: waiting for land service [INFO] [1504534121.070219]: found land service [INFO] [1504534121.070431]: waiting for takeoff service [INFO] [1504534121.071272]: found takeoff service 2. I tried the teleoperation example and there is no error. teleoperation_ps3.launch, sometimes the pink IMU arrow doesn't show up in RVIZ and there is no response if I push the sticks of ps3. If I wait for several minutes, the IMU arrow may show up. Sometimes I can see the IMU arrow and can use left sticks to rotate the moter. But the take off, land button and right stick seem never work. Are the frame figure correct? Do you have any idea how to solve this? Thanks

[whoenig]

Your tf frames look correct. There might be an issue with your PS3 joystick. Try looking at the topic: rostopic echo /crazyflie/joy and see if the output changes if you move the ps3 sticks. Also, for PS3 joysticks you frequently need to press the PS3 button in the middle after you connected them, otherwise they wont work. Similarly, check the output of rostopic echo /crazyflie/imu to see if it is just a visualization issue in rviz, or if there is really no data coming from the CF.

[zhanghaijason]

@whoenig , there is always output when I move the sticks and press the buttons. However, in /teleop_ps3.demo, the CF doesn't turn off the engines no matter which button I press. But there is output from topic /joy. For the imu, when the pink arrow doesn't show up, there is no imu output. Sometimes it may show up, the angular_velocity and linear_acceleration seems correct. But the orientation is always 0, header: seq: 2561 stamp: secs: 1504698842 nsecs: 46585096 frame_id: "crazyflie/base_link" orientation: x: 0.0 y: 0.0 z: 0.0 w: 0.0
In addition, the stuck of CF in RVIZ is because the bad marker positions in which condition the optitrack cannot track CF.

I even tried rosservice call /crazyflie/takeoff, the CF still doesn't take off.

[zhanghaijason]

Hi @whoenig , I bought a new CF and now it can take off. But I only succeed once for the hovering demo. I will try to tune the parameters and to see if that works. Thanks

[zhanghaijason]

Hi @whoenig I have several questions

1. The ROS sometimes gives me the info Size doesn't match!, why is it?
2. For the controller. If the target in CF's body frame is (3,-1,0). Then according to the CF's left hand frame, the roll<0, pitch<0 should be correct. On the other hand, for the y direction in CF's body frame in controller.cpp, linear.y=PID(0,-1), and the error in PID controller is defined as error=-1-0=-1, however, the PID parameter of Y direction are <0, then the linear.y>0, roll=linear.y>0, which is opposite to what it should be(roll<0).

[whoenig]

1. What data do you try to log? I believe this can happen if you try to log too many items per log request (i.e. if the data doesn't fit into the 32 byte acknowledgement packet).
2. In the firmware roll (y) is somewhere inverted for the setpoint; I forgot the exact place, but that is why there is a different sign for pitch vs roll.

[zhanghaijason]

@whoenig Thanks for your explanation. The first question is because I tried to log x,y,z position in CF's body frame and also the control input. In addition, previously I failed many times for the hovering experiment. It is because the markers I use are two heavy for the CF, so it influences the CF's stabilization (Even it is OK to control it with PC Client and Xbox360 controller). Considering my motion tracking system is not that powerful, the CF may fly out of optitrack's vision field. Hope this can help those who also place too heavy markers on CF.

[prabhatkmishra]

@zhanghaijason Please let me know the changes you made in hover_optitrack.launch

[s3xyxavy]

@prbhtm hi, I am also using optitrack as my external positioning system. Would like to help if you have any issues.

[prabhatkmishra]

Thanks, @s3xyxavy. I modofied hover_vicon.launch file correctly. I can give take off and land commands from my joy stick. My crazyflie does not stay at a fixed point but it moves in some direction. Is this because of coordinate mismatch with optitrack and ros?

[s3xyxavy]

1. Put your 4 markers on 3 legs and 1 on top

2. Turn 90 degree left/right and re- Rigid body it on Optitrack

3. Re launch the hover file

[prabhatkmishra]

Thanks @s3xyxavy. Now it rotates and diverges.

[s3xyxavy]

There is a coordinate and orientations mismatch. May I know what streaming node are you using? https://github.com/ros-drivers/mocap_optitrack Should work fine.

[prabhatkmishra]

@s3xyxavy , I am using vrpn client. Unfortunately mocap and natnet don't give any output when I type rostopic echo /crazyflie/pose; rosrun tf view_frames also gives no data received. Initially both were working. Now I am able to connect only by vrpn client. Therefore, I am thinking to focus on vrpn and correct the coordinate and orientation mismatch. DO you have any idea how to do it?

[s3xyxavy]

Access this part of the file: https://github.com/ros-drivers/vrpn_client_ros/blob/kinetic-devel/src/vrpn_client_ros.cpp And have it changed to this Axis conversion. [Take note of the negative sign] Always check your IP address on your VRPN, as it changes once in awhile. Yes and I recommend you use the VRPN client, its more 'editable'. Do rmb to catkin_make, after you make the changes. Hope this helps.

[prabhatkmishra]

@s3xyxavy Thanks a lot. It is still diverging but slowly. I am also getting an error related to link quality [ WARN] [1522860195.066634571]: Link Quality low (0.170000)

I am using ROS-Indigo on ubuntu 14.04 and my crazy radio PA is 0/80/250K . I get link quality warning even if I put crazyflie in front of crazy radio.

[s3xyxavy]

Yea I get that link quality too, but as Long it loads the takeoff emergency and landing you are good to Go.

You should try facing the red led to the positive x of the optitrack and re-rigid body on Optitrack again.

[s3xyxavy]

@prbhtm Hi, try facing your crazyflie 2.0 red led to the positive of Optitrack's X-axis and Re-Rigid body the marker on Optitrack. My convention only works in Y-up on Optitrack.

[prabhatkmishra]

Thank you very much @s3xyxavy .

[podnerogato]

I have a similar question as I want to publish these roll/pitch/yaw control inputs into a node so i can bag them. How to do that? I have accessed and published cmdVelChanged which are said to be the roll/pitch/yaw references in the crazyflie_server.cpp, but these are actually position references(?). Is it possible to publish them from controller.cpp and how? If I am not mistaken, these values are published in m_pubNav which doesn't appear on a rostopic list when using dwm_loc_ekf_hover.launch. I have tried writing a new node but the written publisher doesn't appear on the rostopic list neither.

[whoenig]

The input of crazyflie_controller is a position ("goal" topic), and the output are the roll/pitch angles, yaw rate, and thrust (as a manual pilot would command them).

Note that the crazyflie_controller is outdated. For new projects, you should use the on-board controller that's part of newer firmware releases.

[podnerogato]

EDIT: Managed to log data through cf client. Thanks.

[zhanghaijason]

@zhanghaijason Please let me know the changes you made in hover_optitrack.launch

I am really sorry to reply you so late. I don't have a notification. Sorry. But I am happy to see you solved this.

[gpc668]

hello everyone ! I have some questions when i use the crazyflie_demo ,when i roslaunch crazyflie_demo hover_vrpn.launch, it can linked,but when i use the take off button ,the motors runs but the rotate speed don't enough for the crazyflie to fly. How can i solve the problem?