diff --git a/pps_ws/src/d_fall_pps/CMakeLists.txt b/pps_ws/src/d_fall_pps/CMakeLists.txt
index 9b9d8abeb68c02bd26b6f0bdb651520b66bdc331..0550d2dc9c9f84ecbb5e9aa9cfdb3529cb542dde 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
+ MotorCommand.msg
+ AngleCommand.msg
+ RateCommand.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..782ff3d709f276ee1f4dccbc109f487f75fdab36 100755
--- a/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
+++ b/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
@@ -1,10 +1,9 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
-import roslib
-roslib.load_manifest('d_fall_pps')
+import roslib; roslib.load_manifest('crazypkg')
import rospy
-from d_fall_pps.msg import ControlParameters
+from crazypkg.msg import ControllerOutputPackage
# General import
@@ -29,12 +28,15 @@ from cflib.crazyflie.log import LogConfig
# Logging settings
logging.basicConfig(level=logging.ERROR)
-class SimpleClient:
+CONTROLLER_MOTOR = 2
+CONTROLLER_ANGLE = 1
+CONTROLLER_RATE = 0
+
+class PPSRadioClient:
"""
- 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.
+ CrazyRadio client that recieves the commands from the controller and
+ sends them in a CRTP package to the crazyflie with the specified
+ address.
"""
def __init__(self, link_uri):
@@ -66,64 +68,50 @@ class SimpleClient:
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)
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 "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)
self._cf.send_packet(pk)
- # 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: ")
-
+def motorCommandCallback(data):
+ """Callback for motor controller actions"""
+ cf_client._send_to_commander(0, 0, 0, data.motorCmd1, data.motorCmd2, data.motorCmd3, data.motorCmd4, CONTROLLER_MOTOR)
+ rospy.loginfo("motor controller callback: %s, %s, %s, %s", data.motorCmd1, data.motorCmd2, data.motorCmd3, data.motorCmd4)
+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)
+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)
if __name__ == '__main__':
rospy.init_node('CrazyRadio', anonymous=True)
+ # Initialize the low-level drivers (don't list the debug drivers)
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=[]
@@ -132,20 +120,20 @@ if __name__ == '__main__':
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/80/250K')
- #cf_client = SimpleClient(available[0][0])
- time.sleep(5.0)
- rospy.Subscriber("FlightControl/topicControllerOutput", ControllerOutputPackage, subscriberControllerOutputCallback)
+
+ #TODO: load address from parameters
+ cf_client = PPSRadioClient(available[0][0])
+ time.sleep(3.0)
+ #TODO: change publisher name if not correct
+ rospy.Subscriber("PPSClient/MotorCommands", MotorCommand, motorCommandCallback)
+ rospy.Subscriber("PPSClient/AngleCommands", AngleCommand, angleCommandCallback)
+ rospy.Subscriber("PPSClient/RateCommands", RateCommand, rateCommandCallback)
rospy.spin()
else:
rospy.logerr("No Crazyflies found, cannot run example")
- #inp=raw_input('press any key');
-
time.sleep(0.5)
diff --git a/pps_ws/src/d_fall_pps/launch/ppsLaunch.launch b/pps_ws/src/d_fall_pps/launch/ppsLaunch.launch
new file mode 100644
index 0000000000000000000000000000000000000000..078b17e45a979d5a8b488b18304e8eb5718b06cc
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/launch/ppsLaunch.launch
@@ -0,0 +1,26 @@
+<launch>
+ <!-- Might be needed when our CrazyRadio works. There is no CrazyRadio.launch file yet!
+ <include file="$(find d_fall_pps)/launch/CrazyRadio.launch" />
+ -->
+
+ <node pkg="d_fall_pps" name="ViconDataPublisher" output="screen" type="ViconDataPublisher">
+ </node>
+
+ <node pkg="d_fall_pps" name="PPSClient" output="screen" type="PPSClient">
+ <rosparam command="load" file="$(find d_fall_pps)/launch/studentParams.yaml" />
+ </node>
+
+
+
+ <!-- We will need something similar for our Control node(s)
+ <node pkg="d_fall_pps" name="SafeControllerService" output="screen" type="FlightControl">
+ </node>
+ -->
+
+ <!-- When we have a GUI, this has to be adapted and included
+ <node pkg="d_fall_pps" name="GUI" output="screen" type="GUI">
+ <rosparam command="load" file="$(find d_fall_pps)/launch/ControlParams.yaml" />
+ </node>
+ -->
+
+</launch>
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/launch/studentParams.yaml b/pps_ws/src/d_fall_pps/launch/studentParams.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..8a9c80453c6842622d631c90751aafff50718088
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/launch/studentParams.yaml
@@ -0,0 +1,5 @@
+TeamName: 'Two'
+CrazyFlieName: "cfTwo"
+
+#controllertypes to add with adjustable
+#motor, angle and rate
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/msg/AngleCommand.msg b/pps_ws/src/d_fall_pps/msg/AngleCommand.msg
new file mode 100644
index 0000000000000000000000000000000000000000..22de71ae9568b081ac85557be16a14f78a185ba1
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/msg/AngleCommand.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/MotorCommand.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/MotorCommand.msg
diff --git a/pps_ws/src/d_fall_pps/msg/RateCommand.msg b/pps_ws/src/d_fall_pps/msg/RateCommand.msg
new file mode 100644
index 0000000000000000000000000000000000000000..defddfe07d579957c3edc9f4d99bec0115d5f981
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/msg/RateCommand.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/PPSClient.cpp b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
index bca760e2817eb35bf6e3ad57249cd925cd50f012..7846a81727633dca3ed973dccc7f9e9d759cd3ad 100644
--- a/pps_ws/src/d_fall_pps/src/PPSClient.cpp
+++ b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
@@ -17,23 +17,141 @@
#include "ros/ros.h"
#include "d_fall_pps/ViconData.h"
+//the teamname and the assigned crazyflie, will be extracted from studentParams.yaml
+std::string team; //is this needed?
+std::string cflie;
+
+
+
+//debugging
+int callbackCalls = 0;
+
//is called upon every new arrival of data in main
void viconCallback(const d_fall_pps::ViconData& data){
- ROS_INFO("Callback called");
- ROS_INFO_STREAM(data);
+ //debugging
+ ++callbackCalls;
+ //ROS_INFO("Callback called #%d",callbackCalls);
+ //ROS_INFO("Recived Pitch in this callback: %f", data.pitch);
+ //ROS_INFO("received data:"); ROS_INFO_STREAM(data);
+ //ROS_INFO("My teamname is:"); ROS_INFO_STREAM(team);
+ //ROS_INFO("My crazyflie is:"); ROS_INFO_STREAM(cflie);
+
+ //extract data from "data" and publish/add to service for controller
+ if(data.crazyflieName == cflie){
+ d_fall_pps::ViconData myDataToPublish;
+ myDataToPublish.crazyflieName = data.crazyflieName;
+ myDataToPublish.x = data.x;
+ myDataToPublish.y = data.y;
+ myDataToPublish.z = data.z;
+ myDataToPublish.roll = data.roll;
+ myDataToPublish.pitch = data.pitch;
+ myDataToPublish.yaw = data.yaw;
+ myDataToPublish.acquiringTime = data.acquiringTime;
+ //ROS_INFO("data to share with right controller:");
+ //ROS_INFO_STREAM(myDataToPublish);
+
+
+ //TODO:
+ //Some way of choosing the correct controller: Safe or Custom
+ }
+ else {
+ ROS_INFO("ViconData from other crazyflie received");
+ }
+
+
+}
+
+
+
+
+
+//Send to Crayzradio>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
+void callbackRunRateController(const ros::TimerEvent&)
+{
+ DistributePowerAndSendToCrazyflie(m_pRateController->computeOutput());
+
+}
+
+void DistributePowerAndSendToCrazyflie(ControllerOutput rateControllerOutput)
+{
+ assert(rateControllerOutput.onboardControllerType=eOnboardMotorCmdController);
+ assert(m_isRateOffboard==true);
+
+
+ rateControllerOutput.motorCmd1=m_CrazyControllerOutput.motorCmd1-rateControllerOutput.roll/2.0-rateControllerOutput.pitch/2.0-rateControllerOutput.yaw;
+ rateControllerOutput.motorCmd2=m_CrazyControllerOutput.motorCmd2-rateControllerOutput.roll/2.0+rateControllerOutput.pitch/2.0+rateControllerOutput.yaw;
+ rateControllerOutput.motorCmd3=m_CrazyControllerOutput.motorCmd3+rateControllerOutput.roll/2.0+rateControllerOutput.pitch/2.0-rateControllerOutput.yaw;
+ rateControllerOutput.motorCmd4=m_CrazyControllerOutput.motorCmd4+rateControllerOutput.roll/2.0-rateControllerOutput.pitch/2.0+rateControllerOutput.yaw;
+
+ SendToCrazyflie(rateControllerOutput);
+}
+
+void SendToCrazyflie(ControllerOutput package)
+{
+ if(m_isStopped)
+ {
+ m_packageToSend.motorCmd1=0;
+ m_packageToSend.motorCmd2=0;
+ m_packageToSend.motorCmd3=0;
+ m_packageToSend.motorCmd4=0;
+ m_packageToSend.onboardControllerType=eOnboardMotorCmdController;
+ m_pPublisherControllerOutput->publish(m_packageToSend);
+ return;
+ }
+ else
+ {
+ m_packageToSend.roll=package.roll*RAD2DEG;
+ m_packageToSend.pitch=package.pitch*RAD2DEG;
+ m_packageToSend.yaw=package.yaw*RAD2DEG;
+
+ m_packageToSend.thrust=SaturateToUINT16(package.thrust);
+ m_packageToSend.motorCmd1=SaturateToUINT16(package.motorCmd1);
+ m_packageToSend.motorCmd2=SaturateToUINT16(package.motorCmd2);
+ m_packageToSend.motorCmd3=SaturateToUINT16(package.motorCmd3);
+ m_packageToSend.motorCmd4=SaturateToUINT16(package.motorCmd4);
+ m_packageToSend.onboardControllerType=package.onboardControllerType;
+
+ m_pPublisherControllerOutput->publish(m_packageToSend);
+ }
}
+
+
+//<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
+
int main(int argc, char* argv[]){
ROS_INFO_STREAM("PPSClient started");
ros::init(argc, argv, "PPSClient");
ros::NodeHandle nodeHandle("~");
+ //get the params defined in studentParams.yaml
+ if(!nodeHandle.getParam("TeamName",team)){
+ ROS_ERROR("Failed to get TeamName");
+ }
+
+ if(!nodeHandle.getParam("CrazyFlieName",cflie)){
+ ROS_ERROR("Failed to get CrazyFlieName");
+ }
+
ROS_INFO_STREAM("about to subscribe");
- //maybe set second argument to 1 (as we only want the most recent data)
ros::Subscriber ViconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 1, viconCallback);
ROS_INFO_STREAM("subscribed");
+
+
+ //publish package_for_crazyradio>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
+ //m_pNodeHandle=nodeHandle;
+ //m_pCallbackQueueControlMgr=new ros::CallbackQueue();
+ //m_pNodeHandle->setCallbackQueue(m_pCallbackQueueControlMgr);
+
+
+ ROS_INFO_STREAM("creating publishers for package_for_crazyradio");
+ ros::Pubslisher AngleCommandsPublisher = nodeHandle.advertise <d_fall_pps::AngleCommandsPackage>("AngleCommands", 10));
+ ros::Pubslisher AngleCommandsPublisher = nodeHandle.advertise <d_fall_pps::RateCommandsPackage>("RateCommands", 10));
+ ros::Pubslisher AngleCommandsPublisher = nodeHandle.advertise <d_fall_pps::MotorCommandsPackage>("MotorCommands", 10));
+
+ //<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
ros::spin();
return 0;
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..8c896c49a0e08b7ee9f69ff5af91c459d79ae630 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
+AngleCommand 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..363b6d3c685fa631509d20263eb4f864f6c65f5b
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/srv/MotorController.srv
@@ -0,0 +1,4 @@
+ViconData crazyflieLocation
+Setpoint setpoint
+---
+MotorCommand 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..96469641221aa0aa5f2caa41f1c4aafcb78aa45f
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/srv/RateController.srv
@@ -0,0 +1,4 @@
+ViconData crazyflieLocation
+Setpoint setpoint
+---
+RateCommand controlOutput