diff --git a/pps_ws/src/d_fall_pps/CMakeLists.txt b/pps_ws/src/d_fall_pps/CMakeLists.txt
index 9b9d8abeb68c02bd26b6f0bdb651520b66bdc331..8c96bd4fec9b5840965a5f99ac91b9bde21b5f12 100644
--- a/pps_ws/src/d_fall_pps/CMakeLists.txt
+++ b/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
diff --git a/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py b/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
index a3f818b671a2459593abe22cea63ccf4fe0296e5..67d732fe1e8437d8f6ca8dfc01df2807ce0fd903 100755
--- a/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
+++ b/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:
diff --git a/pps_ws/src/d_fall_pps/crazyradio/CrazyRadioTest.py b/pps_ws/src/d_fall_pps/crazyradio/CrazyRadioTest.py
new file mode 100755
index 0000000000000000000000000000000000000000..7902627b7b56fde48b785a6dfa337e2c0a0b93e0
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/CrazyRadioTest.py
@@ -0,0 +1,160 @@
+#!/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()
+
+
diff --git a/pps_ws/src/d_fall_pps/msg/AngleControl.msg b/pps_ws/src/d_fall_pps/msg/AngleControl.msg
new file mode 100644
index 0000000000000000000000000000000000000000..22de71ae9568b081ac85557be16a14f78a185ba1
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/msg/AngleControl.msg
@@ -0,0 +1,3 @@
+float32 rollAngle
+float32 pitchAngle
+float32 yawAngle
diff --git a/pps_ws/src/d_fall_pps/msg/MotorCommands.msg b/pps_ws/src/d_fall_pps/msg/MotorControl.msg
old mode 100755
new mode 100644
similarity index 100%
rename from pps_ws/src/d_fall_pps/msg/MotorCommands.msg
rename to pps_ws/src/d_fall_pps/msg/MotorControl.msg
diff --git a/pps_ws/src/d_fall_pps/msg/RateControl.msg b/pps_ws/src/d_fall_pps/msg/RateControl.msg
new file mode 100644
index 0000000000000000000000000000000000000000..defddfe07d579957c3edc9f4d99bec0115d5f981
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/msg/RateControl.msg
@@ -0,0 +1,3 @@
+float32 rollRate
+float32 pitchRate
+float32 yawRate
diff --git a/pps_ws/src/d_fall_pps/package.xml b/pps_ws/src/d_fall_pps/package.xml
index 6c0e7c82d7db958b855d0ab51a3eedfa565890ce..ec933d59f8579c17250cfa96afe29a74cc9a710d 100644
--- a/pps_ws/src/d_fall_pps/package.xml
+++ b/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>
diff --git a/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp b/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
index 64c30ef26c30542437cc99d747b5e11b4f8bbc06..20b2bac73b53640c240046848089160b95b41bd5 100644
--- a/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
+++ b/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();
diff --git a/pps_ws/src/d_fall_pps/srv/Controller.srv b/pps_ws/src/d_fall_pps/srv/AngleController.srv
similarity index 64%
rename from pps_ws/src/d_fall_pps/srv/Controller.srv
rename to pps_ws/src/d_fall_pps/srv/AngleController.srv
index 3774e1e30fc12d6ab8f4b052d69cfd22326c9048..7155b47db7c5198f5eeb80a0158b127bf24a46a6 100644
--- a/pps_ws/src/d_fall_pps/srv/Controller.srv
+++ b/pps_ws/src/d_fall_pps/srv/AngleController.srv
@@ -1,4 +1,4 @@
ViconData crazyflieLocation
Setpoint setpoint
---
-MotorCommands controlOutput
+AngleControl controlOutput
diff --git a/pps_ws/src/d_fall_pps/srv/MotorController.srv b/pps_ws/src/d_fall_pps/srv/MotorController.srv
new file mode 100644
index 0000000000000000000000000000000000000000..73167d624648d9e9a6b340b811d95f481005fb66
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/srv/MotorController.srv
@@ -0,0 +1,4 @@
+ViconData crazyflieLocation
+Setpoint setpoint
+---
+MotorControl controlOutput
diff --git a/pps_ws/src/d_fall_pps/srv/RateController.srv b/pps_ws/src/d_fall_pps/srv/RateController.srv
new file mode 100644
index 0000000000000000000000000000000000000000..51e8ceea6cf134b5c26d1071a692c0444eb96593
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/srv/RateController.srv
@@ -0,0 +1,4 @@
+ViconData crazyflieLocation
+Setpoint setpoint
+---
+RateControl controlOutput