more examples + log euler angle + log_script

crazyflie_gazebo/launch/crazyflie_add_multi.launch

@@ -45,8 +45,8 @@
       <rosparam>
         genericLogTopics: ["local_position","target_position"]
         genericLogTopicFrequencies: [10,10]
-        genericLogTopic_pos_Variables: ["stateEstimate.x", "stateEstimate.y", "stateEstimate.z"]
-        genericLogTopic_targetPos_Variables: ["posCtl.targetX", "posCtl.targetY", "posCtl.targetZ"]
+        genericLogTopic_local_position_Variables: ["stateEstimate.x", "stateEstimate.y", "stateEstimate.z", "stabilizer.roll", "stabilizer.pitch", "stabilizer.yaw"]
+        genericLogTopic_target_position_Variables: ["posCtl.targetX", "posCtl.targetY", "posCtl.targetZ", "ctrltarget.roll", "ctrltarget.pitch", "ctrltarget.yaw"]
       </rosparam>
     </node>
   </group>
@@ -71,8 +71,8 @@
       <rosparam>
         genericLogTopics: ["local_position","target_position"]
         genericLogTopicFrequencies: [10,10]
-        genericLogTopic_pos_Variables: ["stateEstimate.x", "stateEstimate.y", "stateEstimate.z"]
-        genericLogTopic_targetPos_Variables: ["posCtl.targetX", "posCtl.targetY", "posCtl.targetZ"]
+        genericLogTopic_local_position_Variables: ["stateEstimate.x", "stateEstimate.y", "stateEstimate.z"]
+        genericLogTopic_target_position_Variables: ["posCtl.targetX", "posCtl.targetY", "posCtl.targetZ"]
       </rosparam>
     </node>
   </group>

crazyflie_gazebo/launch/crazyflie_add_single.launch

@@ -39,8 +39,8 @@
       <rosparam>
         genericLogTopics: ["local_position","target_position"]
         genericLogTopicFrequencies: [10,10]
-        genericLogTopic_pos_Variables: ["stateEstimate.x", "stateEstimate.y", "stateEstimate.z"]
-        genericLogTopic_targetPos_Variables: ["posCtl.targetX", "posCtl.targetY", "posCtl.targetZ"]
+        genericLogTopic_local_position_Variables: ["stateEstimate.x", "stateEstimate.y", "stateEstimate.z", "stabilizer.roll", "stabilizer.pitch", "stabilizer.yaw"]
+        genericLogTopic_target_position_Variables: ["posCtl.targetX", "posCtl.targetY", "posCtl.targetZ", "ctrltarget.roll", "ctrltarget.pitch", "ctrltarget.yaw"]
       </rosparam>
     </node>
   </group>

crazyflie_gazebo/launch/crazyflie_sim.launch

@@ -25,6 +25,7 @@
   <arg name="enable_logging_battery" default="false" />
   <arg name="enable_logging_packets" default="false" />
 
+  <!-- The front propeller are always toward positive x at the beginning -->
   <arg name="tf_prefix_1" default="$(arg cfPrefix)1" />
   <arg name="color_prop_front_1" default="Blue" />
   <arg name="color_prop_back_1" default="Red" />
@@ -36,8 +37,8 @@
   <rosparam>
       genericLogTopics: ["local_position","target_position"]
       genericLogTopicFrequencies: [10,10]
-      genericLogTopic_pos_Variables: ["stateEstimate.x", "stateEstimate.y", "stateEstimate.z"]
-      genericLogTopic_targetPos_Variables: ["posCtl.targetX", "posCtl.targetY", "posCtl.targetZ"]
+      genericLogTopic_local_position_Variables: ["stateEstimate.x", "stateEstimate.y", "stateEstimate.z", "stabilizer.roll", "stabilizer.pitch", "stabilizer.yaw"]
+      genericLogTopic_target_position_Variables: ["posCtl.targetX", "posCtl.targetY", "posCtl.targetZ", "ctrltarget.roll", "ctrltarget.pitch", "ctrltarget.yaw"]
   </rosparam>
 
   <!-- The following line causes gzmsg and gzerr messages to be printed to the console

crazyflie_gazebo/launch/multiple_cf_sim.launch

@@ -55,6 +55,13 @@
   <arg name="y_4" default="1.0" />
   <arg name="z_4" default="0.03" />
 
+  <!-- Some custom blocks for every crazyflie in the environment-->
+  <rosparam>
+      genericLogTopics: ["local_position","target_position"]
+      genericLogTopicFrequencies: [10,10]
+      genericLogTopic_local_position_Variables: ["stateEstimate.x", "stateEstimate.y", "stateEstimate.z", "stabilizer.roll", "stabilizer.pitch", "stabilizer.yaw"]
+      genericLogTopic_target_position_Variables: ["posCtl.targetX", "posCtl.targetY", "posCtl.targetZ", "ctrltarget.roll", "ctrltarget.pitch", "ctrltarget.yaw"]
+  </rosparam>
 
   <!-- The following line causes gzmsg and gzerr messages to be printed to the console
       (even when Gazebo is started through roslaunch) -->

crazyflie_gazebo/scripts/fancy_traj.py

@@ -0,0 +1,176 @@
+#!/usr/bin/env python
+
+import sys
+import rospy
+
+from crazyflie_driver.msg import GenericLogData
+from crazyflie_driver.msg import Position
+
+import math
+
+# Cube around the movement of the crazyflie
+max_x = 1.0
+max_y = 1.0
+max_z = 1.6
+
+# Position in Z for each planar movements
+z_plan = 1.4
+
+############################ Define some trajectories ####################################
+# Define a circular trajectory. Each point returned should be send at the frequency freq
+def circle_trajectory(freq, duration,r):
+	x = list()
+	y = list()
+	t = list()
+	nb_point = freq * duration
+	for i in range(nb_point):
+		t0 = 2 * math.pi * (1.0/freq)  * i
+		x.append(r * math.cos(t0/duration))
+		y.append(r * math.sin(t0/duration))
+		t.append((1.0/freq)  * i)
+	return (t,x,y)
+
+
+# Define a fermat spiral trajectory. Each point returned should be send at the frequency freq
+def fermat_spiral(freq , duration):
+	x = list()
+	y = list()
+	t = list()
+	nb_point = freq * duration
+	for i in range(nb_point):
+		t0 = i * (1.0/freq)
+		x.append((max_x * t0 * t0 * math.cos(t0))/(duration * duration ))
+		y.append((max_y * t0 * t0 * math.sin(t0))/ (duration * duration ))
+		t.append(t0)
+	return (t,x,y)
+
+
+# Define a conical helix trajectory. Each point returned should be send at the frequency freq
+def conical_helix(freq , duration ):
+	x = list()
+	y = list()
+	z = list()
+	t = list()
+	nb_point = freq * duration
+	for i in range(nb_point):
+		t0 = i * (1.0/freq)
+		x.append( (max_x * t0 * math.cos(20 * t0))/duration)
+		y.append( (max_y * t0 * math.sin(20 * t0))/duration)
+		z.append( 0.3 + ((t0/duration) * max_z ))
+		t.append(t0)
+	return (t,x,y,z)
+
+
+# Define a uniform helix trajectory. Each point returned should be send at the frequency freq
+def uniform_helix(freq , duration ):
+	x = list()
+	y = list()
+	z = list()
+	t = list()
+	nb_point = freq * duration
+	for i in range(nb_point):
+		t0 = i * (1.0/freq)
+		x.append( max_x * math.cos(1.0 * t0))
+		y.append( max_y * math.sin(1.0 * t0))
+		z.append( (t0/duration) * max_z )
+		t.append(t0)
+	return (t,x,y,z)
+#########################################################################################
+
+# local position callback [if need]
+def local_position_callback(msg):
+	global current_position
+	current_position.x = msg.values[0]
+	current_position.y = msg.values[1]
+	current_position.z = msg.values[2]
+	current_position.header = msg.header
+
+if __name__ == '__main__':
+
+	rospy.init_node('fancy_traj_node', anonymous=True)
+
+	cf_prefix = "/cf1"
+
+	# SUbscribe to get the local position of the crazyflie with prefix cf_prefix
+	rospy.Subscriber(cf_prefix + "/local_position" , GenericLogData , local_position_callback)
+
+	# The publisher to be able to control in position
+	pubSetpointPos = rospy.Publisher(cf_prefix +"/cmd_position", Position , queue_size=10)
+
+	# Frequency  between each point in the waypoint list
+	freq_pub = 50
+	rate = rospy.Rate(freq_pub)
+
+	# INitialize global variable for actual position of crazyflie
+	current_position = Position()
+
+	# message used to store pos info to send
+	next_pos = Position()
+
+	# Choose a trajectory
+	chosen_traj = raw_input('[ circle spiral conical_helix uniform_helix ]')
+
+	# Takeoff first -> go to 0, 0 , 1
+	while ((0-current_position.x)**2 + (0 - current_position.y)**2 + (1 - current_position.z)**2) > 1e-2 : # 10cm 
+			next_pos.x = 0
+			next_pos.y = 0
+			next_pos.z = 1
+			next_pos.yaw = 0.0
+			next_pos.header.seq += 1
+			next_pos.header.stamp = rospy.Time.now()
+			pubSetpointPos.publish(next_pos)
+			rate.sleep()
+
+	t = list()
+	x = list()
+	y = list()
+	z = list()
+	if chosen_traj == 'circle':
+		(t , x , y) = circle_trajectory(freq_pub , 15  , 1)
+		z = [ z_plan for val in range(len(x))]
+	elif chosen_traj == 'spiral':
+		(t , x , y) = fermat_spiral(freq_pub , 15 )
+		z = [ z_plan for val in range(len(x))]
+	elif chosen_traj == 'conical_helix':
+		(t , x , y , z ) = conical_helix(freq_pub,35)	
+	else :
+		(t , x , y , z ) = uniform_helix(freq_pub,35)
+	for i in range(len(x)):
+		print ("t,x,y,z ", t[i] , x[i], y[i], z[i])
+
+	# first go to the first point of the trajectory
+	while ((x[0]-current_position.x)**2 + (y[0] - current_position.y)**2 + (z[0] - current_position.z)**2) > 1e-2 : # 10cm 
+		next_pos.x = x[0]
+		next_pos.y = y[0]
+		next_pos.z = z[0]
+		next_pos.yaw = 0.0
+		next_pos.header.seq += 1
+		next_pos.header.stamp = rospy.Time.now()
+		pubSetpointPos.publish(next_pos)
+		rate.sleep()
+
+	# Then start the trajectory
+	for i in range(len(x)):
+		next_pos.x = x[i]
+		next_pos.y = y[i]
+		next_pos.z = z[i]
+		next_pos.yaw = 0.0
+		next_pos.header.seq += 1
+		next_pos.header.stamp = rospy.Time.now()
+		pubSetpointPos.publish(next_pos)
+		rate.sleep()
+
+	# Sleep for some time
+	for i in range(100):
+		rate.sleep()
+
+	# Land -> go to 0 0 0.1
+	while ((0-current_position.x)**2 + (0 - current_position.y)**2 + (0.1 - current_position.z)**2) > 1e-2 : # 10cm 
+			next_pos.x = 0
+			next_pos.y = 0
+			next_pos.z = 0.1
+			next_pos.yaw = 0.0
+			next_pos.header.seq += 1
+			next_pos.header.stamp = rospy.Time.now()
+			pubSetpointPos.publish(next_pos)
+			rate.sleep()

crazyflie_gazebo/scripts/run_cfs.sh

@@ -1,5 +1,9 @@
 #!/bin/bash
 
+# First kill current running instances
+pkill cf2
+sleep 0.3
+
 if [ -z "$1" ]
 then
 	max_cfs=1

crazyflie_gazebo/src/crazyflie_ros/joy_control.cpp

@@ -28,6 +28,8 @@
 #define LAND_TOPIC                                      "land"
 #define UPDATE_PARAMS_TOPIC                             "update_params"
 
+#define DEG_2_RAD 3.14159265359/180.0
+
 #define POS_VEL_CONTROL                 3
 #define ACC_ATT_CONTROL                 5     
 
@@ -36,14 +38,14 @@
 #define PI_                   			180.0		//3.1415926535897
 #define PITCH_MAX               		(PI_/20.0)    		//Max pitch angle allowed
 #define ROLL_MAX                		(PI_/20.0)    		//Max roll angle allowed
-#define YAW_MAX_RATE                    (PI_/200.0)           //in deg per seg
+#define YAW_MAX_RATE                    (PI_/3.0)           //in deg per seg
 
 #define THROTTLE_MAX             		55000           	//Max throttle allowed
 
 #define STEP_Z_RATE                     0.05
 #define STEP_Y_X_RATE                   0.05
 #define STEP_ROLL_PITCH                 (PI_/90.0)          //increase the roll and yaw max value by this at every key pressed
-#define STEP_YAW_RATE                   (PI_/40.0)          //increase max yaw_rate by this
+#define STEP_YAW_RATE                   (PI_/10.0)          //increase max yaw_rate by this
 //Joystick configuration
 
 #define THROTTLE_AXE            		1            		//Up/Down left axis
@@ -129,7 +131,8 @@ crazyflie_driver::Hover vel_control_msg;
 
 //Getting feedback from the fcu
 geometry_msgs::Point current_pose;
-// double mc_given_yaw;
+double mc_given_yaw;
+bool contain_yaw = false;
 
 double current_yaw;
 geometry_msgs::Point current_pos;
@@ -229,6 +232,8 @@ void joy_callback(const sensor_msgs::Joy::ConstPtr& msg){
         is_landing = false;
         is_arm = true;
         reset_control_type = true;
+        if (contain_yaw)
+            current_yaw = mc_given_yaw;
         ROS_WARN("POSITION CONTROL ACTIVATED ! ");
     }
 
@@ -294,6 +299,10 @@ void curr_pos_callback(const crazyflie_driver::GenericLogData::ConstPtr& msg){
     current_pose.x = msg->values[0];
     current_pose.y = msg->values[1];
     current_pose.z = msg->values[2];
+    if (msg->values.size() == 6){
+        contain_yaw = true;
+        mc_given_yaw =  msg->values[5] * DEG_2_RAD;
+    }
     // current_pose = msg->pose;
     // mc_given_yaw = tf::getYaw(msg->pose.orientation);
 }
@@ -463,7 +472,10 @@ int main(int argc, char **argv)
     update_params_client.call(m_params);
 
     //setting initial yaw and initial position
-	current_yaw = 0;
+    if (contain_yaw)
+	   current_yaw = mc_given_yaw;
+    else
+        current_yaw = 0;
 	current_pos = current_pose;
 
 	ROS_WARN("[JOY NODE ]:  initial pos : %f , %f , %f ",current_pos.x , current_pos.y , current_pos.z);
@@ -491,7 +503,9 @@ int main(int argc, char **argv)
             current_pos.x += back_forward * max_speed_x * dt.toSec();
             current_pos.y += left_right * max_speed_y * dt.toSec();
             current_pos.z += up_down * max_speed_z * dt.toSec();
-            // current_yaw += yaw * (yaw_max_rate) * dt.toSec();
+            current_yaw += yaw * (yaw_max_rate) * dt.toSec();
+            pos_control_msg.header.seq += 1;
+            pos_control_msg.header.stamp = ros::Time::now();
             pos_control_msg.yaw = current_yaw;
             pos_control_msg.x = current_pos.x;
             pos_control_msg.y = current_pos.y;
@@ -500,18 +514,21 @@ int main(int argc, char **argv)
         }
         if(!only_command && is_velctl){   //Velocity control state
             current_pos.z += up_down * max_speed_z * dt.toSec();
+            vel_control_msg.header.seq += 1;
+            vel_control_msg.header.stamp = ros::Time::now();
             vel_control_msg.vx = back_forward * max_speed_x;
             vel_control_msg.vy = left_right * max_speed_y;
             vel_control_msg.zDistance = current_pos.z; // up_down * max_speed_z;
-            vel_control_msg.yawrate = yaw * (yaw_max_rate);
+            vel_control_msg.yawrate = - yaw * (yaw_max_rate);
+            current_yaw += yaw * (yaw_max_rate) * dt.toSec();
             vel_control_pub.publish(vel_control_msg);
         }
         if(!only_command && is_attctl){       //Attitude control
             att_control_msg.linear.y = - left_right * roll_max;
             att_control_msg.linear.x = back_forward * pitch_max;
-            // current_yaw += yaw * (yaw_max_rate) * dt.toSec();
+            current_yaw += yaw * (yaw_max_rate) * dt.toSec();
             att_control_msg.linear.z = up_down * throttle_max ;
-            att_control_msg.angular.z = current_yaw;
+            att_control_msg.angular.z = - yaw * (yaw_max_rate);
             att_control_pub.publish(att_control_msg);
         }
 

crazyflie_gazebo/src/gazebo_cfGhost_plugin.cpp

@@ -1,6 +1,7 @@
 #include "gazebo_cfGhost_plugin.h"
 
 // #include <tf/transform_datatypes.h>
+#define DEG_2_RAD 3.14159265359/180.0
 
 namespace gazebo {
 
@@ -57,7 +58,10 @@ void GazeboCfGHostPlugin::poseReceivedCallback(const crazyflie_driver::GenericLo
 
 	/*last_pose=ignition::math::Pose3d(position->pose.position.x , position->pose.position.y , position->pose.position.z, 
 		position->pose.orientation.w , position->pose.orientation.x , position->pose.orientation.y , position->pose.orientation.z);*/
-	last_pose=ignition::math::Pose3d(position->values[0] , position->values[1] , position->values[2], 1.0, 0,0,0);
+	if (position->values.size() == 3)
+		last_pose=ignition::math::Pose3d(position->values[0] , position->values[1] , position->values[2], 1.0, 0,0,0);
+	else if (position->values.size() == 6)
+		last_pose=ignition::math::Pose3d(position->values[0] , position->values[1] , position->values[2], position->values[3] * DEG_2_RAD, position->values[4] * DEG_2_RAD, position->values[5]* DEG_2_RAD);
 }
 
 }