Added messages and services for the controller types

pps_ws/src/d_fall_pps/CMakeLists.txt

@@ -12,6 +12,7 @@ find_package(catkin REQUIRED COMPONENTS
   roscpp
   rospy
   std_msgs
+  genmsg
 )
 
 ## System dependencies are found with CMake's conventions
@@ -57,7 +58,9 @@ find_package(catkin REQUIRED COMPONENTS
 add_message_files(
   FILES
   ViconData.msg
-  MotorCommands.msg
+  MotorControl.msg
+  AngleControl.msg
+  RateControl.msg
   Setpoint.msg
 )
 
@@ -69,7 +72,9 @@ add_message_files(
 # )
 add_service_files(
   FILES
-  Controller.srv
+  MotorController.srv
+  AngleController.srv
+  RateController.srv
 )
 
 ## Generate actions in the 'action' folder

pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py

@@ -68,6 +68,7 @@ class SimpleClient:
         """
         #print "Connection to %s successful: " % link_uri
         rospy.loginfo("Connection to %s successful: " % link_uri)
+        cf_client._send_to_commander(1, 1, 1, 100, 100, 100, 100, 100, 1)
 
     def _connection_failed(self, link_uri, msg):
         """Callback when connection initial connection fails (i.e no Crazyflie
@@ -100,7 +101,7 @@ class SimpleClient:
     def _send_to_commander(self,roll, pitch, yaw, thrust,cmd1,cmd2,cmd3,cmd4,mode):
         pk = CRTPPacket()
         pk.port = CRTPPort.COMMANDER
-        pk.data = struct.pack('<fffHHHHHH', roll, pitch, yaw, thrust,cmd1,cmd2,cmd3,cmd4,mode)
+        pk.data = struct.pack('<fffH', roll, pitch, yaw, thrust)
         self._cf.send_packet(pk)
 
         # self._cf.commander.send_setpoint (roll, pitch, yaw, thrust,cmd1,cmd2,cmd3,cmd4,mode)
@@ -135,10 +136,10 @@ if __name__ == '__main__':
             # uri would can be specified directly, as for example: radio://0/70/250K
             # instead of available[0][0]
             global cf_client
-            cf_client = SimpleClient('radio://0/80/250K')
-            #cf_client = SimpleClient(available[0][0])
+            #cf_client = SimpleClient('radio://0/80/250K')
+            cf_client = SimpleClient(available[0][0])
             time.sleep(5.0)
-            rospy.Subscriber("FlightControl/topicControllerOutput", ControllerOutputPackage, subscriberControllerOutputCallback)
+            #rospy.Subscriber("FlightControl/topicControllerOutput", ControllerOutputPackage, subscriberControllerOutputCallback)
 
             rospy.spin()
         else:

pps_ws/src/d_fall_pps/crazyradio/CrazyRadioTest.py

+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+import roslib
+roslib.load_manifest('d_fall_pps')
+import rospy
+from d_fall_pps.msg import ControlParameters
+
+
+# General import
+import time, sys
+import struct
+import logging
+
+# Add library
+#sys.path.append("lib")
+
+# CrazyFlie client imports
+import cflib
+
+from cflib.crazyflie import Crazyflie
+from cflib.crtp.crtpstack import CRTPPacket, CRTPPort
+
+import cflib.drivers.crazyradio
+
+# Logging import
+from cflib.crazyflie.log import LogConfig
+
+# Logging settings
+logging.basicConfig(level=logging.ERROR)
+
+class SimpleClient:
+    """
+       Example script that runs several threads that read Vicon measurements
+       from a file and send it together with the setpoints to the Crazyflie.
+       It also employs a keyboard event detector that allows the user to
+       manipulate the setpoints with keys.
+    """
+    def __init__(self, link_uri):
+
+        # Setpoints to be sent to the CrazyFlie
+        self.roll = 0.0
+        self.pitch = 0.0
+        self.yaw = 0.0
+        self.motor1cmd = 0.0
+        self.motor2cmd = 0.0
+        self.motor3cmd = 0.0
+        self.motor4cmd = 0.0
+
+        # Initialize the CrazyFlie and add callbacks
+        self._cf = Crazyflie()
+
+        # Add callbacks that get executed depending on the connection status.
+        self._cf.connected.add_callback(self._connected)
+        self._cf.disconnected.add_callback(self._disconnected)
+        self._cf.connection_failed.add_callback(self._connection_failed)
+        self._cf.connection_lost.add_callback(self._connection_lost)
+
+        # Connect to the Crazyflie
+        print "Connecting to %s" % link_uri
+        self._cf.open_link(link_uri)
+
+
+    def _connected(self, link_uri):
+        """
+            This callback is executed as soon as the connection to the
+            quadrotor is established.
+        """
+        #print "Connection to %s successful: " % link_uri
+        rospy.loginfo("Connection to %s successful: " % link_uri)
+        cf_client._send_to_commander(0, 0, 0, 1000,    0, 0, 0, 0, 0)
+
+    def _connection_failed(self, link_uri, msg):
+        """Callback when connection initial connection fails (i.e no Crazyflie
+        at the specified address)"""
+        #print "Connection to %s failed: %s" % (link_uri, msg)
+        rospy.logerr("Connection to %s failed: %s" % (link_uri, msg))
+
+    def _connection_lost(self, link_uri, msg):
+        """Callback when disconnected after a connection has been made (i.e
+        Crazyflie moves out of range)"""
+        #print "Connection to %s lost: %s" % (link_uri, msg)
+        rospy.logerr("Connection to %s lost: %s" % (link_uri, msg))
+
+    def _disconnected(self, link_uri):
+        """Callback when the Crazyflie is disconnected (called in all cases)"""
+        #print "Disconnected from %s" % link_uri
+        rospy.logwarn("Disconnected from %s" % link_uri)
+
+
+    def _send_commands(self,cmd1,cmd2,cmd3,cmd4):
+        # Send setpoints at the given frequency.
+        # Fill the CRTP packet with the setpoints and send it to the stabilizer
+        pk = CRTPPacket()
+        pk.port = CRTPPort.STABILIZER
+        pk.data = struct.pack('<ffff', cmd1,cmd2,cmd3,cmd4)
+        self._cf.send_packet(pk)
+        print('command')
+        #print "Motor commands: %f, %f, %f, %f" % (cmd1,cmd2,cmd3,cmd4)
+
+    def _send_to_commander(self,roll, pitch, yaw, thrust,cmd1,cmd2,cmd3,cmd4,mode):
+        pk = CRTPPacket()
+        pk.port = CRTPPort.COMMANDER
+        #pk.data = struct.pack('<fffHHHHHH', roll, pitch, yaw, thrust,cmd1,cmd2,cmd3,cmd4,mode)
+        pk.data = struct.pack('<fffH', roll, pitch, yaw, thrust)
+        self._cf.send_packet(pk)
+        print(thrust)
+
+        # self._cf.commander.send_setpoint (roll, pitch, yaw, thrust,cmd1,cmd2,cmd3,cmd4,mode)
+        # print "Motor commands: %f, %f, %f, %f" % (cmd1,cmd2,cmd3,cmd4)
+
+def subscriberControllerOutputCallback(data):
+    #cf_client._send_commands(data.cmd1,data.cmd2,data.cmd3,data.cmd4)
+    cf_client._send_to_commander(data.roll,data.pitch,data.yaw,data.thrust,data.motorCmd1, data.motorCmd2, data.motorCmd3, data.motorCmd4, data.onboardControllerType)
+    #rospy.loginfo(data.onboardControllerType);
+    #rospy.loginfo(data.motorCmd1);
+    #rospy.logdebug_throttle(2,"sending motor commands to crazyflie: ")
+
+
+
+
+if __name__ == '__main__':
+    rospy.init_node('CrazyRadio', anonymous=True)
+    cflib.crtp.init_drivers(enable_debug_driver=False)
+
+    while not rospy.is_shutdown():
+
+        # Initialize the low-level drivers (don't list the debug drivers)
+
+        # Scan for Crazyflies and use the first one found
+        rospy.loginfo("Scanning interfaces for Crazyflies...")
+        available=[]
+        available = cflib.crtp.scan_interfaces()
+        rospy.loginfo("Crazyflies found:")
+        for i in available:
+            print i[0]
+        if len(available) > 0:
+            # uri would can be specified directly, as for example: radio://0/70/250K
+            # instead of available[0][0]
+            global cf_client
+            #cf_client = SimpleClient('radio://0/111/250K')
+            cf_client = SimpleClient(available[0][0])
+            time.sleep(5.0)
+            #rospy.Subscriber("FlightControl/topicControllerOutput", ControllerOutputPackage, subscriberControllerOutputCallback)
+
+            while True:
+                cf_client._send_to_commander(1, 1, 1, 1000,    100, 100, 100, 100, 100)
+
+            rospy.spin()
+        else:
+            rospy.logerr("No Crazyflies found, cannot run example")
+
+        #inp=raw_input('press any key');
+
+
+        time.sleep(0.5)
+
+    cf_client._cf.close_link()
+
+

pps_ws/src/d_fall_pps/msg/AngleControl.msg

+float32 rollAngle
+float32 pitchAngle
+float32 yawAngle

pps_ws/src/d_fall_pps/msg/RateControl.msg

+float32 rollRate
+float32 pitchRate
+float32 yawRate

pps_ws/src/d_fall_pps/package.xml

@@ -43,6 +43,7 @@
   <build_depend>roscpp</build_depend>
   <build_depend>rospy</build_depend>
   <build_depend>std_msgs</build_depend>
+  <build_depend>genmsg</build_depend>
   <run_depend>roscpp</run_depend>
   <run_depend>rospy</run_depend>
   <run_depend>std_msgs</run_depend>

pps_ws/src/d_fall_pps/src/SafeControllerService.cpp

@@ -15,12 +15,12 @@
 //    along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
 #include "ros/ros.h"
-#include "d_fall_pps/Controller.h"
+#include "d_fall_pps/RateController.h"
 #include "d_fall_pps/ViconData.h"
 #include "d_fall_pps/Setpoint.h"
-#include "d_fall_pps/MotorCommands.h"
+#include "d_fall_pps/MotorControl.h"
 
-bool calculateControlOutput(d_fall_pps::Controller::Request &request, d_fall_pps::Controller::Response &response) {
+bool calculateControlOutput(d_fall_pps::RateController::Request &request, d_fall_pps::RateController::Response &response) {
     ROS_INFO("calculate control output");
 }
 
@@ -29,7 +29,7 @@ int main(int argc, char* argv[]) {
 
     ros::NodeHandle nodeHandle("~");
 
-    ros::ServiceServer service = nodeHandle.advertiseService("Controller", calculateControlOutput);
+    ros::ServiceServer service = nodeHandle.advertiseService("RateController", calculateControlOutput);
     ROS_INFO("SafeControllerService ready");
     ros::spin();
 

pps_ws/src/d_fall_pps/srv/Controller.srv → pps_ws/src/d_fall_pps/srv/AngleController.srv

 ViconData crazyflieLocation
 Setpoint setpoint
 ---
-MotorCommands controlOutput
+AngleControl controlOutput

pps_ws/src/d_fall_pps/srv/MotorController.srv

+ViconData crazyflieLocation
+Setpoint setpoint
+---
+MotorControl controlOutput

pps_ws/src/d_fall_pps/srv/RateController.srv

+ViconData crazyflieLocation
+Setpoint setpoint
+---
+RateControl controlOutput