From e6ed9ee6dff78ae400d804c4ccba07e00af00e66 Mon Sep 17 00:00:00 2001
From: muelmarc <muelmarc@student.ethz.ch>
Date: Fri, 7 Apr 2017 17:10:06 +0200
Subject: [PATCH] added area and controlleroutput structs to PPSClient for
testing; added CentralManagerService to launch file
---
.../src/d_fall_pps/crazyradio/CrazyRadio.py | 2 +-
pps_ws/src/d_fall_pps/launch/ppsLaunch.launch | 4 +
.../src/d_fall_pps/launch/studentParams.yaml | 2 +-
.../d_fall_pps/src/CentralManagerService.cpp | 12 ++
pps_ws/src/d_fall_pps/src/PPSClient.cpp | 141 ++++++++++++++----
5 files changed, 132 insertions(+), 29 deletions(-)
diff --git a/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py b/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
index 3d628118..86d82df6 100755
--- a/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
+++ b/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
@@ -94,7 +94,7 @@ class PPSRadioClient:
def motorCommandCallback(data):
"""Callback for motor controller actions"""
- cf_client._send_to_commander(0, 0, 0, 0, data.cmd1, data.cmd2 * 1000, data.cmd3 * 1000, data.cmd4 * 1000, CONTROLLER_MOTOR)
+ cf_client._send_to_commander(0, 0, 0, 0, data.cmd1, data.cmd2, data.cmd3, data.cmd4, CONTROLLER_MOTOR)
rospy.loginfo("motor controller callback: %s, %s, %s, %s", data.cmd1, data.cmd2, data.cmd3, data.cmd4)
def angleCommandCallback(data):
diff --git a/pps_ws/src/d_fall_pps/launch/ppsLaunch.launch b/pps_ws/src/d_fall_pps/launch/ppsLaunch.launch
index bb283c22..c47d076f 100644
--- a/pps_ws/src/d_fall_pps/launch/ppsLaunch.launch
+++ b/pps_ws/src/d_fall_pps/launch/ppsLaunch.launch
@@ -1,4 +1,7 @@
<launch>
+
+ <node pkg="d_fall_pps" name="CentralManagerService" output="screen" type="CentralManagerService">
+ </node>
<node pkg="d_fall_pps" name="CrazyRadio" output="screen" type="CrazyRadio.py">
<rosparam command="load" file="$(find d_fall_pps)/launch/studentParams.yaml" />
@@ -15,6 +18,7 @@
<node pkg="d_fall_pps" name="SafeControllerService" output="screen" type="SafeControllerService">
</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" />
diff --git a/pps_ws/src/d_fall_pps/launch/studentParams.yaml b/pps_ws/src/d_fall_pps/launch/studentParams.yaml
index 739ad896..a9845472 100644
--- a/pps_ws/src/d_fall_pps/launch/studentParams.yaml
+++ b/pps_ws/src/d_fall_pps/launch/studentParams.yaml
@@ -1,5 +1,5 @@
TeamName: 'Two'
-CrazyFlieName: "cfTwo"
+CrazyFlieName: "cfFour"
CrazyFlieAddress: "radio://0/99/2M"
SafeController: ""
SafeControllerType: ""
diff --git a/pps_ws/src/d_fall_pps/src/CentralManagerService.cpp b/pps_ws/src/d_fall_pps/src/CentralManagerService.cpp
index 2cb409de..98a52e98 100644
--- a/pps_ws/src/d_fall_pps/src/CentralManagerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/CentralManagerService.cpp
@@ -25,6 +25,18 @@ using namespace d_fall_pps;
//send them back the CrazyflieContext (containing (maybe among other things) the area to fly in)
bool returnCrazyflieContext(CentralManager::Request &request, CentralManager::Response &response) {
ROS_INFO("central manager");
+
+
+ //TBD: crazyflie-dependent area assignment instead of hardcoding
+ //request contains string crazyflieName
+ //respond with area boundaries upon request
+ response.context.localArea.xmin = -1000;
+ response.context.localArea.xmax = 1000;
+ response.context.localArea.ymin = -1000;
+ response.context.localArea.ymax = 1000;
+ response.context.localArea.zmin = -200;
+ response.context.localArea.zmax = 800;
+
return true;
}
diff --git a/pps_ws/src/d_fall_pps/src/PPSClient.cpp b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
index 276b2824..d174e138 100644
--- a/pps_ws/src/d_fall_pps/src/PPSClient.cpp
+++ b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
@@ -24,8 +24,15 @@
#include "ros/ros.h"
-#include "d_fall_pps/ViconData.h"
+
+//include autogenerated headers from srv files
#include "d_fall_pps/RateController.h"
+#include "d_fall_pps/AngleController.h"
+#include "d_fall_pps/MotorController.h"
+#include "d_fall_pps/CentralManager.h"
+
+//include autogenerated headers from msg files
+#include "d_fall_pps/ViconData.h"
#include "d_fall_pps/AngleCommand.h"
#include "d_fall_pps/RateCommand.h"
#include "d_fall_pps/MotorCommand.h"
@@ -40,6 +47,7 @@ std::string cflie;
//global sevices
ros::ServiceClient rateClient;
+ros::ServiceClient centralClient;
ros::Publisher angleCommandPublisher;
ros::Publisher rateCommandPublisher;
@@ -49,21 +57,55 @@ ros::Publisher motorCommandPublisher;
//ros::NodeHandle nodeHandle;
-//acceptance test for crazyflie position and attitude
-bool safetyCheck(ViconData data){
- CrazyflieContext CrazyflieContext;
-
- return true;
-}
+//move this section to a separate file that is included>>>>>>>>>
+struct AreaBoundaries{
+ float areaBoundxmin;
+ float areaBoundxmax;
+ float areaBoundymin;
+ float areaBoundymax;
+ float areaBoundzmin;
+ float areaBoundzmax;
+};
+
+AreaBoundaries Area;
+
+
+//struct consistent with dusans controller
+struct ControllerOutput
+{
+ //ControllerOutput():roll(0),pitch(0),yaw(0),thrust(0),motorCmd1(0),motorCmd2(0),motorCmd3(0),motorCmd4(0) {}
+ float roll;
+ float pitch;
+ float yaw;
+ float thrust;
+ float motorCmd1;
+ float motorCmd2;
+ float motorCmd3;
+ float motorCmd4;
+ uint8_t onboardControllerType;
+};
+
+ControllerOutput ControlCommandTest;
+//<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<move this section to a separate file that is included
//extract data from "data" and publish/add to service for controller
//not void: sould give back controlldata
-void ppsClientToController(ViconData data, bool autocontrol){
+void ppsClientToController(ViconData data, bool autocontrolOn){
if(data.crazyflieName == cflie){
- //TODO:
- //Some way of choosing the correct controller: Safe or Custom
- //using the area data
+ //call safecontroller if autocontrol is true
+ if(autocontrolOn){
+ ControlCommandTest.motorCmd1 = 0;
+ ControlCommandTest.motorCmd2 = 0;
+ ControlCommandTest.motorCmd3 = 0;
+ ControlCommandTest.motorCmd4 = 0;
+ }
+ else {
+ ControlCommandTest.motorCmd1 = 1000;
+ ControlCommandTest.motorCmd2 = 1000;
+ ControlCommandTest.motorCmd3 = 1000;
+ ControlCommandTest.motorCmd4 = 1000;
+ }
//TODO:
//communicating with Controller
@@ -94,24 +136,37 @@ void ppsClientToController(ViconData data, bool autocontrol){
}
}
-
-//debugging
-//int callbackCalls = 0;
+//acceptance test for crazyflie position and attitude
+bool safetyCheck(ViconData data){
+ CrazyflieContext CrazyflieContext;
+ //ROS_INFO("in safetyCheck");
+ //ROS_INFO_STREAM("ViconData: " << data.x << ", " << data.y << ", " << data.z);
+
+ //position check
+
+ //attitude check
+
+ //all checks passed
+ return false;
+}
//is called upon every new arrival of data in main
void viconCallback(const d_fall_pps::ViconData& data){
- //debugging
- //++callbackCalls;
- //ROS_INFO("Callback called #%d",callbackCalls);
- //ROS_INFO("Received Pitch in this callback: %f", data.pitch);
- //ROS_INFO("received data:"); ROS_INFO_STREAM(data);
+ ROS_INFO("in viconCallback");
+ //ROS_INFO_STREAM(data);
//ROS_INFO("My teamname is:"); ROS_INFO_STREAM(team);
//ROS_INFO("My crazyflie is:"); ROS_INFO_STREAM(cflie);
- //forward data to safety check
- bool autocontrolOn = safetyCheck(data);
+ if(data.crazyflieName == cflie){
+ //forward data to safety check
+ bool autocontrolOn = safetyCheck(data);
+ ROS_INFO_STREAM("autocontrolOn: " << autocontrolOn);
+ ppsClientToController(data, autocontrolOn);
+ }
+ else {
+ ROS_INFO("ViconData from other crazyflie received");
+ }
- ppsClientToController(data, autocontrolOn);
}
@@ -147,10 +202,10 @@ void callbackRateCommand(const ros::TimerEvent&)
void callbackMotorCommand(const ros::TimerEvent&)
{
d_fall_pps::MotorCommand motorCommandPkg;
- motorCommandPkg.cmd1 = 3;
- motorCommandPkg.cmd2 = 3;
- motorCommandPkg.cmd3 = 3;
- motorCommandPkg.cmd4 = 3;
+ motorCommandPkg.cmd1 = ControlCommandTest.motorCmd1;
+ motorCommandPkg.cmd2 = ControlCommandTest.motorCmd2;
+ motorCommandPkg.cmd3 = ControlCommandTest.motorCmd3;
+ motorCommandPkg.cmd4 = ControlCommandTest.motorCmd4;
motorCommandPublisher.publish(motorCommandPkg);
ROS_INFO_STREAM("MotorCommandTimer pubslishes: " << motorCommandPkg.cmd1 << ", " << motorCommandPkg.cmd2 << ", " << motorCommandPkg.cmd3 << ", " << motorCommandPkg.cmd4);
@@ -196,10 +251,42 @@ int main(int argc, char* argv[]){
motorCommandPublisher = nodeHandle.advertise <d_fall_pps::MotorCommand>("MotorCommand", 1);
- //service: now only one available: to add several services depending on controller
+ //service
+ //to be expanded with additional services depending on controller (currently only one available)
rateClient = nodeHandle.serviceClient<d_fall_pps::RateController>("/SafeControllerService/RateController");
+
+ //rateClient = nodeHandle.serviceClient<d_fall_pps::RateController>("/SafeControllerService/RateController", true);
+ //http://wiki.ros.org/roscpp/Overview/Services
+ //2.1 Persistenct Connection: ROS also allows for persistent connections to services. With a persistent connection, a client stays connected to a service.
+ // Otherwise, a client normally does a lookup and reconnects to a service each time.
+
+ //service
+ centralClient = nodeHandle.serviceClient<d_fall_pps::CentralManager>("/CentralManagerService/CentralManager");
+
+
+ //TBD: some sort of init procedure to get data from CentralManager upfront
+ //this is only for testing>>>>>>>>>>>>>>>>>>>>>>>>>>>>
+ CentralManager ManagerSettings;
+ if(centralClient.call(ManagerSettings)){
+
+ Area.areaBoundxmin = ManagerSettings.response.context.localArea.xmin;
+ Area.areaBoundxmax = ManagerSettings.response.context.localArea.xmin;
+ Area.areaBoundymin = ManagerSettings.response.context.localArea.xmin;
+ Area.areaBoundymax = ManagerSettings.response.context.localArea.xmin;
+ Area.areaBoundzmin = ManagerSettings.response.context.localArea.xmin;
+ Area.areaBoundzmax = ManagerSettings.response.context.localArea.xmin;
+ ROS_INFO("CentralManager responded");
+ ROS_INFO("AreaBoundaries Set");
+
+ }
+ else{
+ ROS_ERROR("Failed to call CentralManagerService. Callback is aborted");
+ //return some useful stuff
+ return 0;
+ }
+ //<<<<<<<<<<<<<<<<<<<<<<<this is only for testing
ros::spin();
return 0;
--
GitLab