ROS package modifications, Crazyflie receives the wrong orientation
Posted: Fri Jul 20, 2018 7:46 pm
Hi everyone, I'm following this report:
https://arxiv.org/pdf/1608.05786.pdf
Like the guys, I modified the server script and the controller script in c++, in particular they created an user interactive topic named "/goal" created in MATLAB that lets the user choose between a number of predefined trajectories for the quadcopter to follow. Once the trajectory is selected, through a certain ID number that can be changed in real-time, the MATLAB node publishes the desired waypoint [xc, yc, zc, ψc]. An additional feature allows the user to choose whether to send position or velocity commands to the yaw angle of the drone.
I tried to implement the same things, and everything worked fine, the problem occurs when I tried to send some position to this topic from Matlab. The drone seems to not be able to see the position because he flies but with the orientation completely wrong. For example in the trajectory generation I send some position in x,y,z and yaw, the /goal topic can be published correctly and I can see the values that I send for example [x:0, y:0, z:0.5, yaw:0], but the drone do something else like if it has "no eyes". Can someone help me to find the problem?
I'm using Ubuntu 16.04 and LPS. This is my catkin workspace:
https://drive.google.com/open?id=171ppa ... CaIGPXXy8
the controllerPID.cpp and the serverPID.cpp are in this google drive folder in src/crazyflie_ros/crazyflie_controller or /crazyflie_server
In MATLAB I can see every value of the sensors, so I think it depends on the controller.cpp script or server.cpp script.
and this is my launch file:
Launch file:
Thank you for helping me!!!
https://arxiv.org/pdf/1608.05786.pdf
Like the guys, I modified the server script and the controller script in c++, in particular they created an user interactive topic named "/goal" created in MATLAB that lets the user choose between a number of predefined trajectories for the quadcopter to follow. Once the trajectory is selected, through a certain ID number that can be changed in real-time, the MATLAB node publishes the desired waypoint [xc, yc, zc, ψc]. An additional feature allows the user to choose whether to send position or velocity commands to the yaw angle of the drone.
I tried to implement the same things, and everything worked fine, the problem occurs when I tried to send some position to this topic from Matlab. The drone seems to not be able to see the position because he flies but with the orientation completely wrong. For example in the trajectory generation I send some position in x,y,z and yaw, the /goal topic can be published correctly and I can see the values that I send for example [x:0, y:0, z:0.5, yaw:0], but the drone do something else like if it has "no eyes". Can someone help me to find the problem?
I'm using Ubuntu 16.04 and LPS. This is my catkin workspace:
https://drive.google.com/open?id=171ppa ... CaIGPXXy8
the controllerPID.cpp and the serverPID.cpp are in this google drive folder in src/crazyflie_ros/crazyflie_controller or /crazyflie_server
In MATLAB I can see every value of the sensors, so I think it depends on the controller.cpp script or server.cpp script.
and this is my launch file:
Launch file:
Code: Select all
<?xml version="1.0"?>
<launch>
<arg name="uri" default="radio://0/100/2M/E7E7E7E7E7" />
<arg name="frame" default="base_link" />
<arg name="worldFrame" default="world"/>
<arg name="joy_dev" default="/dev/input/js0" />
<arg name="x" default="2.8" />
<arg name="y" default="2.3" />
<arg name="z" default="1" />
<param name="robot_description" command="$(find xacro)/xacro.py $(find crazyflie_description)/urdf/crazyflie2.urdf.xacro" />
<group ns="crazyflie">
<rosparam command="load" file="$(find bitcraze_lps_estimator)/data/anchor_pos.yaml" />
<node pkg="crazyflie_driver" type="crazyflie_add" name="crazyflie_add" output="screen">
<param name="uri" value="$(arg uri)" />
<param name="tf_prefix" value="crazyflie" />
<rosparam>
genericLogTopics: ["log_kfpos", "log_kfqt", "log_ranges"]
genericLogTopicFrequencies: [30, 30, 30]
genericLogTopic_log_kfpos_Variables: ["kalman.stateX", "kalman.stateY", "kalman.stateZ"]
genericLogTopic_log_kfqt_Variables: ["kalman.q0", "kalman.q1", "kalman.q2", "kalman.q3"]
genericLogTopic_log_ranges_Variables: ["ranging.distance0", "ranging.distance1", "ranging.distance2", "ranging.distance3", "ranging.distance4", "ranging.distance5", "ranging.state"]
</rosparam>
</node>
<node name="controller" pkg="crazyflie_controller" type="crazyflie_controller" output="screen">
<param name="frame" value="$(arg frame)" />
<param name="worldFrame" value="$(arg worldFrame)" />
<rosparam command="load" file="$(find crazyflie_controller)/config/crazyflie2.yaml" />
</node>
<node name="joy" pkg="joy" type="joy_node" output="screen">
<param name="dev" value="$(arg joy_dev)" />
</node>
<node name="controller_bridge" pkg="bitcraze_lps_estimator" type="crazyflie_controller_bridge.py" output="screen"/>
<node name="pose" pkg="crazyflie_demo" type="publish_pose_teleop.py" output="screen">
<param name="name" value="goal" />
<param name="rate" value="30" />
<param name="x" value="$(arg x)" />
<param name="y" value="$(arg y)" />
<param name="z" value="$(arg z)" />
</node>
<node name="lps_efk_bridge" pkg="bitcraze_lps_estimator" type="lps_ekf_bridge.py" output="screen"/>
<node name="lps_viz" pkg="bitcraze_lps_estimator" type="lps_viz.py" />
<node name="log_range" pkg="bitcraze_lps_estimator" type="log_range.py" />
</group>
<node pkg="tf" type="static_transform_publisher" name="link1_broadcaster"
args="1 0 0 0 0 0 1 world lps 100" />
<include file="$(find crazyflie_driver)/launch/crazyflie_server.launch" />
</launch>
Thank you for helping me!!!