ROS package modifications, Crazyflie receives the wrong orientation

Discussions about autonomous flight in general, regardless of positioning method
Post Reply
flavia1394
Beginner
Posts: 23
Joined: Fri Mar 30, 2018 5:10 pm

ROS package modifications, Crazyflie receives the wrong orientation

Post by flavia1394 »

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
Schermata 2018-07-20 alle 3.34.34 PM.png
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!!!
whoenig
Expert
Posts: 395
Joined: Mon Oct 27, 2014 2:55 am

Re: ROS package modifications, Crazyflie receives the wrong orientation

Post by whoenig »

It is pretty difficult to see which part is changed and which part is default:
1. Do you run the default firmware? If not, what are the firmware changes? In any case, which exact firmware are you running?
2. Which changes did you do to crazyflie_ros?

Also, I am unclear on what the exact behavior is - is flying in the wrong direction, or is the orientation simply wrong?
flavia1394
Beginner
Posts: 23
Joined: Fri Mar 30, 2018 5:10 pm

Re: ROS package modifications, Crazyflie receives the wrong orientation

Post by flavia1394 »

whoenig wrote: Mon Jul 23, 2018 10:33 pm It is pretty difficult to see which part is changed and which part is default:
1. Do you run the default firmware? If not, what are the firmware changes? In any case, which exact firmware are you running?
2. Which changes did you do to crazyflie_ros?

Also, I am unclear on what the exact behavior is - is flying in the wrong direction, or is the orientation simply wrong?
Thank you so much for your answer:
1) I run the latest firmware, so the 2018.01, In the firmware I changed two things because the body-fixed frame defined in the embedded system did not match the one adopted during the simulation phase: the first is the power_distribution_stock.c
Schermata 2018-07-24 alle 10.29.28 AM.png
and the second is the controller_pid.c
Schermata 2018-07-24 alle 10.29.35 AM.png
2) My changes in the crazyflie ros package are this:
  • in the crazyflie controller.cpp I created new subscribers:

    m_imu_sub (topic /crazyflie/imu): displays information coming from the IMU of the Crazyflie 2.0, in particular the gyroscope measurements and then the Euler angles (roll, pitch, yaw) coming from the Sensor Fusion algorithm embedded in the Crazyflie.

    -m_lpspos_sub (topic /crazyflie/crazyflie_position): subscribes to the position estimations of the LPS system.

    -m_goal_sub (topic /goal): subscribes to the data coming from the MATLAB node. When using the PID controller it represents the Euler angles setpoints.

    Code: Select all

    m_goal_sub = nh.subscribe("/goal", 1000, &Controller::GoalCallback, this);
    m_imu_sub = nh.subscribe("/crazyflie/imu", 1000, &Controller::imuCallback, this);
    m_lpspos_sub = nh.subscribe("/crazyflie/crazyflie_position", 1000, &Controller::posLpsCallback, this);
      
    the methods related to the subscribers are this:

    Code: Select all

    void GoalCallback( const geometry_msgs::Twist::ConstPtr& msg)
    	{ 
    
    		goal_x = msg->linear.x+x_0;    //X Setpoint
    		goal_y = msg->linear.y+y_0;    //Y Setpoint
    		goal_z = msg->linear.z;        //Z Setpoint
    		goal_select = msg->angular.x;  //Trajectory Selector
    		yaw_mode = msg->angular.y;     //Select Yaw mode: angular or rate
    		goal_yaw = msg->angular.z;     //Yaw Setpoint
    		}
    void posLpsCallback( const geometry_msgs::PoseStamped::ConstPtr& msg)
    	{
    
    		actual_x = msg->pose.position.x;
    		actual_y = msg->pose.position.y;
    		actual_z = msg->pose.position.z;
    
    	}	
    void imuCallback(const sensor_msgs::Imu::ConstPtr& msg)
    	{
    		actual_roll = msg->angular_velocity.x*3.141592654/180;
    		actual_pitch = msg->angular_velocity.y*3.141592654/180;
    		actual_yaw = msg->angular_velocity.z*3.141592654/180;
    	}	
    	
    And this is the iteration Method:

    Code: Select all

    void iteration(const ros::TimerEvent& e)
    	{
    		float dt = e.current_real.toSec() - e.last_real.toSec();
    		float pi = 3.141592654;
    
    
    		switch(goal_select) //Selected via MATLAB interface
    		{
    		case 0:  //Mode Idle, do nothing and await new commands
    		{	//ROS_INFO("MODE IDLE");
    			geometry_msgs::Twist msg;
    			if(i==0 && actual_x!=0){ //Save initial coordinates as point (0,0)
    				x_0=actual_x; //x_0, y_0, z_0 come from from MATLAB
    				y_0=actual_y;
    				i++;
    			}
    
    			pidReset();
    			msg.linear.x =0;
    			msg.linear.y =0;
    			msg.linear.z=0;
    			msg.angular.z = 0;
    		m_pubNav.publish(msg);
    		}
    		break;
    
    
    		case 1: //Landing on demand
    		{
    			//ROS_INFO("Landing");
    			geometry_msgs::Twist msg;
    			if(land_req==0 && actual_x!=0){ //Take current coordinates as landing site
    				x_0=actual_x;
    				y_0=actual_y;
    				z_0=actual_z;
    				land_req++;
    			}
    			m_thrust =45000 + m_pidZ.update(actual_z , z_0+goal_z);
    			if(actual_z>0.25){
    				d_x = (cos(actual_yaw) * (actual_x-old_x) + sin(actual_yaw) * (actual_y-old_y))/0.01;
    				d_y = (-sin(actual_yaw) * (actual_x-old_x) + cos(actual_yaw) * (actual_y-old_y))/0.01;
    				old_x = actual_x;
    				old_y = actual_y;
    				//First we take the difference between the last value of X-Y and the present value
    				//Then we do the appropriate rotation if we have some yaw angle to align the axis
    				xy_error[0] =  cos(actual_yaw) * (x_0 - actual_x) + sin(actual_yaw) * (y_0 - actual_y);
    				xy_error[1] = -sin(actual_yaw) * (x_0 - actual_x) + cos(actual_yaw) * (y_0 - actual_y);
    				msg.linear.x =m_pidX.update(d_x, xy_error[0]);
    				msg.linear.y =m_pidY.update(d_y, xy_error[1]);
    				msg.linear.z=m_thrust;
    				msg.angular.z = m_pidYaw.update(actual_yaw*180/pi,0);
    			}
    
    			else {
    				msg.linear.x =0;
    				msg.linear.y =0;
    				msg.linear.z=0;
    				msg.angular.z = 0;
    			}
    			if (m_state==1){
    			ROS_INFO("Emergency Stop");
    			msg.linear.x = 0;
    			msg.linear.y = 0;
    			msg.linear.z = 0;
    			msg.angular.z = 0;
    		}
    		m_pubNav.publish(msg);
    		}
    		break;
    
    
    		default: //Default for any trajectory other than take-off and landing
    		{
    			//ROS_INFO("Following a Trajectory");
    			geometry_msgs::Twist msg;
    			m_thrust =45000 + m_pidZ.update(actual_z , goal_z);
    			if(actual_z>0.25){  //After take-off
    				d_x = (cos(actual_yaw) * (actual_x-old_x) + sin(actual_yaw) * (actual_y-old_y))/0.01;
    				d_y = (-sin(actual_yaw) * (actual_x-old_x) + cos(actual_yaw) * (actual_y-old_y))/0.01;
    				old_x = actual_x;
    				old_y = actual_y;
    				//First we take the difference between the last value of X-Y and the present value
    				//Then we do the appropriate rotation if we have some yaw angle to align the axis
    				xy_error[0] =  cos(actual_yaw) * (goal_x - actual_x) + sin(actual_yaw) * (goal_y - actual_y);
    				xy_error[1] = -sin(actual_yaw) * (goal_x - actual_x) + cos(actual_yaw) * (goal_y - actual_y);
    				//This PID takes like input the difference between the error in x-y
    				//and maps it to a desired velocity, which is compared with the actual linear velocity
    				msg.linear.x =m_pidX.update(d_x, xy_error[0]);
    				msg.linear.y =m_pidY.update(d_y, xy_error[1]);
    				msg.linear.z=m_thrust;
    				if(yaw_mode==0) //Yaw angular mode
    					msg.angular.z = m_pidYaw.update(actual_yaw*180/pi,goal_yaw);
    				else  //Yaw rate mode
    					msg.angular.z=goal_yaw;
    
    			}
    			else{  //Before Take-off
    				msg.linear.z=45000;
    				msg.linear.x = 0;
    				msg.linear.y = 0;
    				msg.angular.z = 0;
    			}
    			if (m_state==1){
    			ROS_INFO("Emergency Stop");
    			msg.linear.x = 0;
    			msg.linear.y = 0;
    			msg.linear.z = 0;
    			msg.angular.z = 0;
    		}
    		m_pubNav.publish(msg);
    		}
    		break;
    
    		}
    
    	} 
  • The modification to the crazyflie_server.cpp are this:

    Publishers:

    -m_pubImu (topic /crazyflie/imu): IMU data from the Crazyflie in degrees(Euler angles) and degrees per second(Gyroscope readings).

    -m_pubMotors (topic /crazyflie/motor): publishes the PWM signals sent to the motors (16 bit value). (not used in this case, I wanted to use them in the LQ controller).

Code: Select all

struct logMotor {
		int m1;
		int m2;
		int m3;
		int m4;
	} __attribute__((packed));
	
	void cmdVelChanged(
    const geometry_msgs::Twist::ConstPtr& msg)
	{
		if (!m_isEmergency) {
			float roll = msg->linear.y + m_roll_trim;
			float pitch =  msg->linear.x + m_pitch_trim;
			float yawrate = msg->angular.z;
			uint16_t thrust = std::min<uint16_t>(std::max<float>(msg->linear.z, 0.0), 60000);

			m_cf.sendSetpoint(roll, pitch, yawrate, thrust);
			m_sentSetpoint = true;
		}
	}
	
	void run()
  {
    ros::NodeHandle n;
    n.setCallbackQueue(&m_callback_queue);

    m_subscribeCmdVel = n.subscribe(m_tf_prefix + "/cmd_vel", 1, &CrazyflieROS::cmdVelChanged, this);
   .........
   .........
    m_pubMotors = n.advertise<std_msgs::Int32MultiArray>(m_tf_prefix + "/motor", 1000); //*****new********
    
    }
    std::function<void(uint32_t, logMotor*)> cbmotor = std::bind(&CrazyflieROS::onMotorData, this, std::placeholders::_1,std::placeholders::_2);

	logBlockMotor.reset(new LogBlock<logMotor>(
			&m_cf,{
							{"motor", "m1"},
							{"motor", "m2"},
							{"motor", "m3"},
							{"motor", "m4"},
			}, cbmotor));
			logBlockMotor->start(1); // 10ms

      }

.......

void onImuData(uint32_t time_in_ms, logImu* data) {
    if (m_enable_logging_imu) {
      sensor_msgs::Imu msg;
      if (m_use_ros_time) {
        msg.header.stamp = ros::Time::now();
      } else {
        msg.header.stamp = ros::Time(time_in_ms / 1000.0);
      }
      msg.header.frame_id = m_tf_prefix + "/base_link";
      msg.orientation_covariance[0] = -1;

                msg.orientation.x = cos(data->yaw/2)*cos(data->pitch/2)*cos(data->roll/2) + sin(data->yaw/2)*sin(data->pitch/2)*sin(data->roll/2);
		msg.orientation.y = sin(data->yaw/2)*cos(data->pitch/2)*cos(data->roll/2) - cos(data->yaw/2)*sin(data->pitch/2)*sin(data->roll/2);
		msg.orientation.z = cos(data->yaw/2)*sin(data->pitch/2)*cos(data->roll/2) + sin(data->yaw/2)*cos(data->pitch/2)*sin(data->roll/2);
		msg.orientation.w = cos(data->yaw/2)*cos(data->pitch/2)*sin(data->roll/2) - sin(data->yaw/2)*sin(data->pitch/2)*cos(data->roll/2);

		// measured in deg/s; need to convert to rad/s
		msg.angular_velocity.x = data->roll;//degToRad(data->roll);
		msg.angular_velocity.y = data->pitch;//degToRad(data->pitch);
		msg.angular_velocity.z = data->yaw;//degToRad(data->yaw);

		// measured in mG; need to convert to m/s^2
		msg.linear_acceleration.x = data->gyro_x* 9.81;
		msg.linear_acceleration.y = data->gyro_y* 9.81;
		msg.linear_acceleration.z = data->gyro_z* 9.81;

		m_pubImu.publish(msg);
    }
  }

void onMotorData(uint32_t time_in_ms, logMotor* data) {
		std_msgs::Int32MultiArray array;
		array.data.clear();
		array.data.push_back(data->m1);
		array.data.push_back(data->m2);
		array.data.push_back(data->m3);
		array.data.push_back(data->m4);

		m_pubMotors.publish(array);
	}
Now, everything compiles perfectly, the topic are published and when I run the MATLAB script the propellers of the crazyflie start to run, the problem is that it seems that the crazyflie has not a controller at all. For example here:
Schermata 2018-07-24 alle 11.00.24 AM.png
I just want that the crazyflie goes in (x:0,y:0,z:0.5) but it starts to fly in any direction like if there is no controller and there is no position sensor, I don't think it's a MATLAB problem because in the subscribers blocks I can see perfectly the values of all the sensors.
This is inside the LPS block:
Schermata 2018-07-24 alle 11.02.29 AM.png
and this inside the trajectory generation:

Code: Select all

function [x,y,z,yaw_mode,yaw] = message(t,select)
x = 0;
y= 0;
z = 0;
yaw_mode = 0; % 0 for Position mode, 1 for Rate mode
yaw = 0; %If position mode, then this is the setpoint in deg
         %If rate mode, then the setpoint is in deg/s
global t0; 
switch(select) 
    case 1 %PROTECTED case, left unchanged for landing
        x= 0;
        y= 0;
        z= -0.1*(t-t0); %specify rate in cm/s for descending
        yaw_mode = 0;
        yaw=0;   
               
    case 2 % Hover 0.5 meter altitude @ initial position
        
        if(t<12)
        x = 0; %The initial position is always added ROS side
        y = 0; %Same here, x_0 and y_0 are automatically added in ROS  
        z = 0.5;
        yaw_mode = 0;
        yaw = 0;
        end
        
        if(t>12 & t<25)
        x = 0;
        y = 0;
        z = 0.5;
        yaw_mode = 0;
        yaw = 0;
        end
        
        if(t>25)
        x = 0;
        y = 0; 
        z = 0.5;
        yaw_mode = 0;
        yaw = 0;
        end
          
end

The behavior is that even if i use this values, the drone goes flies randomly. I was thinking that maybe there are some problem with that initial conditions x_0, y_0, z:0, or I need to do some other changes since I have an LPS system of bitcraze and not a VICON system. But I don't know exactly what change I should do.

Thank you for this help! I want to do the formation of three drones, but of course I need to let fly one first, and I got stuck here.
whoenig
Expert
Posts: 395
Joined: Mon Oct 27, 2014 2:55 am

Re: ROS package modifications, Crazyflie receives the wrong orientation

Post by whoenig »

Did you test your overall setup of the UWB using stock/unchanged software? One issue (as you point out) might be the coordinate system, that 0,0,0 is not where you expect it is. Using the UWB you won't get as good tracking performance (because the state estimate will be noisier) compared to VICON, but what you try to do should still work.

Finally, is your work on trajectory generation or control? The control methods you use are not the best: there is a much better trajectory tracking controller running on-board now. If you run the controller off-board you won't achieve as good results as running on-board.
flavia1394
Beginner
Posts: 23
Joined: Fri Mar 30, 2018 5:10 pm

Re: ROS package modifications, Crazyflie receives the wrong orientation

Post by flavia1394 »

whoenig wrote: Wed Jul 25, 2018 5:49 am Did you test your overall setup of the UWB using stock/unchanged software? One issue (as you point out) might be the coordinate system, that 0,0,0 is not where you expect it is. Using the UWB you won't get as good tracking performance (because the state estimate will be noisier) compared to VICON, but what you try to do should still work.

Finally, is your work on trajectory generation or control? The control methods you use are not the best: there is a much better trajectory tracking controller running on-board now. If you run the controller off-board you won't achieve as good results as running on-board.
Yes I checked my setup before the modification and everything worked well. Also, I used the python scripts to let the drone follow a waypoint and it worked too. The only thing that it doesn't work is my ROS controller with the modifications, when I run the MATLAB script it seems that the drone doesn't have a controller actually. I don't know if I should modify something else.

Anyway, what I would like to do is to let the drone follow some trajectories that I send from MATLAB. So, first of all I want simply use the PID and let it follow a random trajectory, and after that, I want to use LQT to control just the position. The main purpose is a trajectory tracking.

I saw that the cmd_vel topic is made of Roll,Pitch,Yaw in degrees and thrust in PWM. Is it possible (if I use the original ROS package), sending some trajectories to follow? for example a simple circle trajectory or something like that to send to the cmd_vel topic.

Thank you for your patient with me and for your work! I really appreciate that.
whoenig
Expert
Posts: 395
Joined: Mon Oct 27, 2014 2:55 am

Re: ROS package modifications, Crazyflie receives the wrong orientation

Post by whoenig »

For trajectory tracking, the recommended way is to use the nonlinear controller that runs on-board (you can switch to it by changing the "stabilizer.controller" parameter to 2; might require latest master-firmware). Then, you can publish topics to "cmd_full_state", that includes position, velocity, acceleration, and angular velocity as setpoint.
It is also possible to upload trajectories while the CF is on the ground (in form of polynomials) and then just execute without any additional radio communication. Finally, there is now a planner on-board that can plan smooth trajectories for way-point following, see https://github.com/whoenig/crazyflie_ro ... h_level.py for an example.

Unfortunately, there is not much documentation on this yet, but it's high up on our to do list!
flavia1394
Beginner
Posts: 23
Joined: Fri Mar 30, 2018 5:10 pm

Re: ROS package modifications, Crazyflie receives the wrong orientation

Post by flavia1394 »

whoenig wrote: Thu Jul 26, 2018 1:11 am For trajectory tracking, the recommended way is to use the nonlinear controller that runs on-board (you can switch to it by changing the "stabilizer.controller" parameter to 2; might require latest master-firmware). Then, you can publish topics to "cmd_full_state", that includes position, velocity, acceleration, and angular velocity as setpoint.
It is also possible to upload trajectories while the CF is on the ground (in form of polynomials) and then just execute without any additional radio communication. Finally, there is now a planner on-board that can plan smooth trajectories for way-point following, see https://github.com/whoenig/crazyflie_ro ... h_level.py for an example.

Unfortunately, there is not much documentation on this yet, but it's high up on our to do list!
Thank you for this answer, I tried to run:

Code: Select all

sudo python test_high_level.py
but this is the problem:
Schermata 2018-08-10 alle 1.29.40 PM.png

I tried:
Schermata 2018-08-10 alle 1.31.13 PM.png
What could be the problem?
whoenig
Expert
Posts: 395
Joined: Mon Oct 27, 2014 2:55 am

Re: ROS package modifications, Crazyflie receives the wrong orientation

Post by whoenig »

Looks like ROS is not correctly installed. Did you install it yourself or are you using some kind of pre-configured system? Also, you shouldn't run normal code (such as this testing script) using sudo. If you get permission issues, you have to add the proper udev rules for the Crazyradio for your normal user account.
Post Reply