tried to get CrazyRadio running

pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py

@@ -94,23 +94,20 @@ class PPSRadioClient:
 
 def motorCommandCallback(data):
     """Callback for motor controller actions"""
-    rospy.loginfo("test motorCommandCallback")
+    cf_client._send_to_commander(0, 0, 0, 0, data.cmd1, data.cmd2, data.cmd3, 40, CONTROLLER_MOTOR)
+    rospy.loginfo("motor controller callback: %s, %s, %s, %s", data.cmd1,data.cmd2,data.cmd3,data.cmd4)
 
 def angleCommandCallback(data):
     """Callback for angle controller actions"""
-    ###cf_client._send_to_commander(data.roll,data.pitch,data.yaw,data.thrust, 0, 0, 0, 0, CONTROLLER_ANGLE)
-    ####rospy.loginfo("angle controller callback: %s, %s, %s", data.roll,data.pitch,data.yaw,data.thrust)
-    rospy.loginfo("test angleCommandCallback")
+    #cmd1..4 must not be 0, as crazyflie onboard controller resets
+    #cf_client._send_to_commander(data.rollAngle,data.pitchAngle,data.yawAngle,data.thrust, 1, 1, 1, 1, CONTROLLER_ANGLE)
+    rospy.loginfo("angle controller callback: %s, %s, %s, %s", data.rollAngle, data.pitchAngle ,data.yawAngle, data.thrust)
 
 def rateCommandCallback(data):
     """Callback for rate controller actions"""
-    ###cf_client._send_to_commander(data.roll,data.pitch,data.yaw,data.thrust, 0, 0, 0, 0, CONTROLLER_RATE)
-    ###rospy.loginfo("rate controller callback: %s, %s, %s", data.roll,data.pitch,data.yaw,data.thrust)
-    rospy.loginfo("test rateCommandCallback")
-
-def testCallback(data):
-    """Callback used to test data receipt if no crazyfly was found"""
-    rospy.loginfo("Crazyradio.py successfully subscribed and received testvalues: %f", data.cmd1)
+    #cmd1..4 must not be 0, as crazyflie onboard controller resets
+    #cf_client._send_to_commander(data.rollRate,data.pitchRate,data.yawRate,data.thrust, 1, 1, 1 , 1, CONTROLLER_RATE)
+    rospy.loginfo("rate controller callback: %s, %s, %s, %s", data.rollRate, data.pitchRate, data.yawRate, data.thrust)
 
 if __name__ == '__main__':
     rospy.init_node('CrazyRadio', anonymous=True)
@@ -119,7 +116,6 @@ if __name__ == '__main__':
 
 
     while not rospy.is_shutdown():
-
         # Scan for Crazyflies and use the first one found
         rospy.loginfo("Scanning interfaces for Crazyflies...")
         available=[]
@@ -132,25 +128,15 @@ if __name__ == '__main__':
 
             #TODO: load address from parameters
             cf_client = PPSRadioClient(available[0][0])
-            time.sleep(3.0)
+            time.sleep(2.0)
             #TODO: change publisher name if not correct
-            rospy.Subscriber("PPSClient/topicMotorCommand", MotorCommand, motorCommandCallback)
-            rospy.loginfo("trying to subscribe")
-            #rospy.Subscriber("PPSClient/AngleCommands", AngleCommand, angleCommandCallback)
-            #rospy.Subscriber("PPSClient/RateCommands", RateCommand, rateCommandCallback)
+            rospy.Subscriber("/PPSClient/MotorCommand", MotorCommand, motorCommandCallback)
+            rospy.Subscriber("/PPSClient/AngleCommand", AngleCommand, angleCommandCallback)
+            rospy.Subscriber("/PPSClient/RateCommand", RateCommand, rateCommandCallback)
 
             rospy.spin()
+            rospy.loginfo("Setting crazyflie setpoint to 0")
+            cf_client._send_to_commander(0, 0, 0, 0, 0, 0, 0, 0, 0)
+            cf_client._cf.close_link()
         else:
-            #rospy.logerr("No Crazyflies found, cannot run example")
-            #for testing try to subscribe even if no crazyflie was found
-            rospy.loginfo("No Crazyflies found, still trying to subscribe")
-            rospy.Subscriber("PPSClient/topicMotorCommand", MotorCommand, testCallback)
-            rospy.spin()
-
-
-
-        time.sleep(0.5)
-
-    cf_client._cf.close_link()
-
-
+            rospy.logerr("No Crazyflies found")

pps_ws/src/d_fall_pps/msg/AngleCommand.msg

 float32 rollAngle
 float32 pitchAngle
 float32 yawAngle
+int32 thrust

pps_ws/src/d_fall_pps/msg/MotorCommand.msg

-float32 cmd1
-float32 cmd2
-float32 cmd3
-float32 cmd4
+int32 cmd1
+int32 cmd2
+int32 cmd3
+int32 cmd4
 

pps_ws/src/d_fall_pps/msg/RateCommand.msg

 float32 rollRate
 float32 pitchRate
 float32 yawRate
+int32 thrust

pps_ws/src/d_fall_pps/src/PPSClient.cpp

@@ -39,9 +39,9 @@ std::string cflie;
 //global sevices
 ros::ServiceClient rateClient;
 
-ros::Publisher AngleCommandPublisher;
-ros::Publisher RateCommandPublisher;
-ros::Publisher MotorCommandPublisher;
+ros::Publisher angleCommandPublisher;
+ros::Publisher rateCommandPublisher;
+ros::Publisher motorCommandPublisher;
 
 //uncommenting the next line causes FATAL Error at runtime: "You must call ros::init() before creating the first NodeHandle"
 //ros::NodeHandle nodeHandle;
@@ -121,38 +121,40 @@ void viconCallback(const d_fall_pps::ViconData& data){
 //callback method to publish d_fall_pps::AngleCommand
 void callbackAngleCommand(const ros::TimerEvent&)
 {
-	d_fall_pps::AngleCommand AngleCommandPkg;
-	AngleCommandPkg.rollAngle = 1;
-	AngleCommandPkg.pitchAngle = 1;
-	AngleCommandPkg.yawAngle = 1;
+	d_fall_pps::AngleCommand angleCommandPkg;
+	angleCommandPkg.rollAngle = 1;
+	angleCommandPkg.pitchAngle = 1;
+	angleCommandPkg.yawAngle = 1;
+	angleCommandPkg.thrust = 50;
 	
-	AngleCommandPublisher.publish(AngleCommandPkg);
-	ROS_INFO_STREAM("AngleCommandTimer pubslishes: " << AngleCommandPkg.rollAngle << ", " << AngleCommandPkg.pitchAngle << ", " << AngleCommandPkg.yawAngle);
+	angleCommandPublisher.publish(angleCommandPkg);
+	ROS_INFO_STREAM("AngleCommandTimer pubslishes: " << angleCommandPkg.rollAngle << ", " << angleCommandPkg.pitchAngle << ", " << angleCommandPkg.yawAngle);
 }
 
 //callback method to publish d_fall_pps::RateCommand
 void callbackRateCommand(const ros::TimerEvent&)
 {
-	d_fall_pps::RateCommand RateCommandPkg;
-	RateCommandPkg.rollRate = 2;
-	RateCommandPkg.pitchRate = 2;
-	RateCommandPkg.yawRate = 2;
+	d_fall_pps::RateCommand rateCommandPkg;
+	rateCommandPkg.rollRate = 2;
+	rateCommandPkg.pitchRate = 2;
+	rateCommandPkg.yawRate = 2;
+	rateCommandPkg.thrust = 50;
 	
-	RateCommandPublisher.publish(RateCommandPkg);
-	ROS_INFO_STREAM("RateCommandTimer pubslishes: " << RateCommandPkg.rollRate << ", " << RateCommandPkg.pitchRate << ", " << RateCommandPkg.yawRate);
+	rateCommandPublisher.publish(rateCommandPkg);
+	ROS_INFO_STREAM("RateCommandTimer pubslishes: " << rateCommandPkg.rollRate << ", " << rateCommandPkg.pitchRate << ", " << rateCommandPkg.yawRate);
 }
 
 //callback method to publish d_fall_pps::MotorCommand
 void callbackMotorCommand(const ros::TimerEvent&)
 {
-	d_fall_pps::MotorCommand MotorCommandPkg;
-	MotorCommandPkg.cmd1 = 3;
-	MotorCommandPkg.cmd2 = 3;
-	MotorCommandPkg.cmd3 = 3;
-	MotorCommandPkg.cmd4 = 3;
+	d_fall_pps::MotorCommand motorCommandPkg;
+	motorCommandPkg.cmd1 = 3;
+	motorCommandPkg.cmd2 = 3;
+	motorCommandPkg.cmd3 = 3;
+	motorCommandPkg.cmd4 = 3;
 	
-	MotorCommandPublisher.publish(MotorCommandPkg);
-	ROS_INFO_STREAM("MotorCommandTimer pubslishes: " << MotorCommandPkg.cmd1 << ", " << MotorCommandPkg.cmd2 << ", " << MotorCommandPkg.cmd3 << ", " << MotorCommandPkg.cmd4);
+	motorCommandPublisher.publish(motorCommandPkg);
+	ROS_INFO_STREAM("MotorCommandTimer pubslishes: " << motorCommandPkg.cmd1 << ", " << motorCommandPkg.cmd2 << ", " << motorCommandPkg.cmd3 << ", " << motorCommandPkg.cmd4);
 }
 
 
@@ -184,15 +186,15 @@ int main(int argc, char* argv[]){
 	Reference: http://wiki.ros.org/roscpp/Overview/Timers
 	*/
     ROS_INFO("creating publishers for package_for_crazyradio");
-	ros::Timer AngleCommandTimer = nodeHandle.createTimer(ros::Duration(0.1), callbackAngleCommand);
-	ros::Timer RateCommandTimer = nodeHandle.createTimer(ros::Duration(0.1), callbackRateCommand);
-	ros::Timer MotorCommandTimer = nodeHandle.createTimer(ros::Duration(0.1), callbackMotorCommand);
+	ros::Timer angleCommandTimer = nodeHandle.createTimer(ros::Duration(0.1), callbackAngleCommand);
+	ros::Timer rateCommandTimer = nodeHandle.createTimer(ros::Duration(0.1), callbackRateCommand);
+	ros::Timer motorCommandTimer = nodeHandle.createTimer(ros::Duration(0.1), callbackMotorCommand);
 	
 	
 	//ros::Publishers to advertise on the three command type topics
-	AngleCommandPublisher = nodeHandle.advertise <d_fall_pps::AngleCommand>("topicAngleCommand", 1000);
-	RateCommandPublisher = nodeHandle.advertise<d_fall_pps::RateCommand>("topicRateCommand", 1000);
-	MotorCommandPublisher = nodeHandle.advertise <d_fall_pps::MotorCommand>("topicMotorCommand", 1000);
+	angleCommandPublisher = nodeHandle.advertise <d_fall_pps::AngleCommand>("AngleCommand", 1000);
+	rateCommandPublisher = nodeHandle.advertise<d_fall_pps::RateCommand>("RateCommand", 1000);
+	motorCommandPublisher = nodeHandle.advertise <d_fall_pps::MotorCommand>("MotorCommand", 1000);
 
 
 	//service: now only one available: to add several services depending on controller

pps_ws/src/d_fall_pps/src/SafeControllerService.cpp

@@ -45,6 +45,7 @@ bool calculateControlOutput(RateController::Request &request, RateController::Re
     response.controlOutput.rollRate = 1; //testvalue
 	response.controlOutput.pitchRate = 2; //testvalue
 	response.controlOutput.yawRate = 3; //testvalue
+    response.controlOutput.thrust = 50;
 
 	return true;
 }