diff --git a/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py b/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
index e137fc0cb603102f906743ff323b27698b1e8d76..ee4f562eacc45f0e96834f0cc14e62ace5c85180 100755
--- a/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
+++ b/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")
diff --git a/pps_ws/src/d_fall_pps/msg/AngleCommand.msg b/pps_ws/src/d_fall_pps/msg/AngleCommand.msg
index 22de71ae9568b081ac85557be16a14f78a185ba1..dbe4a850e46dde66b609a92f7bb2430d2353fec1 100644
--- a/pps_ws/src/d_fall_pps/msg/AngleCommand.msg
+++ b/pps_ws/src/d_fall_pps/msg/AngleCommand.msg
@@ -1,3 +1,4 @@
float32 rollAngle
float32 pitchAngle
float32 yawAngle
+int32 thrust
diff --git a/pps_ws/src/d_fall_pps/msg/MotorCommand.msg b/pps_ws/src/d_fall_pps/msg/MotorCommand.msg
index 1ba103f54d977ce9620e123de107800ee6714973..c221dc22b0e60636e474550d68206992aa1377cc 100644
--- a/pps_ws/src/d_fall_pps/msg/MotorCommand.msg
+++ b/pps_ws/src/d_fall_pps/msg/MotorCommand.msg
@@ -1,5 +1,5 @@
-float32 cmd1
-float32 cmd2
-float32 cmd3
-float32 cmd4
+int32 cmd1
+int32 cmd2
+int32 cmd3
+int32 cmd4
diff --git a/pps_ws/src/d_fall_pps/msg/RateCommand.msg b/pps_ws/src/d_fall_pps/msg/RateCommand.msg
index defddfe07d579957c3edc9f4d99bec0115d5f981..aa85c008395095330bf2b2127828e12467ca8ccc 100644
--- a/pps_ws/src/d_fall_pps/msg/RateCommand.msg
+++ b/pps_ws/src/d_fall_pps/msg/RateCommand.msg
@@ -1,3 +1,4 @@
float32 rollRate
float32 pitchRate
float32 yawRate
+int32 thrust
diff --git a/pps_ws/src/d_fall_pps/src/PPSClient.cpp b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
index 28de6d08c3a7bbbd120c39c64ed40fb57e28408e..7f24b704a3672d5b3b347d436c3c1ae116f68235 100644
--- a/pps_ws/src/d_fall_pps/src/PPSClient.cpp
+++ b/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
diff --git a/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp b/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
index e56560cf26df04ab1172d7886bcb65f50a4ac583..485b2318d0a6f83dffef08fdc8eeb3d5d356a62c 100644
--- a/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
+++ b/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;
}