Merge branch 'pps_project' of https://gitlab.ethz.ch/D-FaLL/D-FaLL-System into pps_project

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

pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py

 #!/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)
 

pps_ws/src/d_fall_pps/launch/ppsLaunch.launch

+<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

pps_ws/src/d_fall_pps/launch/studentParams.yaml

+TeamName: 'Two'
+CrazyFlieName: "cfTwo"
+
+#controllertypes to add with adjustable
+#motor, angle and rate
\ No newline at end of file

pps_ws/src/d_fall_pps/msg/AngleCommand.msg

+float32 rollAngle
+float32 pitchAngle
+float32 yawAngle

pps_ws/src/d_fall_pps/msg/RateCommand.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/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;

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
+AngleCommand controlOutput

pps_ws/src/d_fall_pps/srv/MotorController.srv

+ViconData crazyflieLocation
+Setpoint setpoint
+---
+MotorCommand controlOutput

pps_ws/src/d_fall_pps/srv/RateController.srv

+ViconData crazyflieLocation
+Setpoint setpoint
+---
+RateCommand controlOutput