Commit e921a7a3 authored by bucyril's avatar bucyril
Browse files

PPSClient can receive commands from terminal

parent aa0bba96
......@@ -104,8 +104,8 @@ if __name__ == '__main__':
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
if rospy.has_param("/CrazyRadio/CrazyFlieAddress"):
radio_address = rospy.get_param("/CrazyRadio/CrazyFlieAddress")
if rospy.has_param("/CrazyRadio/crazyFlieAddress"):
radio_address = rospy.get_param("/CrazyRadio/crazyFlieAddress")
rospy.loginfo("Crazyradio connecting to %s" % radio_address)
global cf_client
......
TeamName: 'Two'
CrazyFlieName: "cfFour"
CrazyFlieAddress: "radio://0/99/2M"
SafeController: ""
SafeControllerType: ""
CustomController: ""
CustomControllerType: ""
teamName: "Two"
crazyFlieName: "cfFour"
crazyFlieAddress: "radio://0/99/2M"
safeController: "/SafeControllerService/RateController"
customController: ""
#controllertypes to add with adjustable
#motor, angle and rate
#!/bin/bash
if [ "$#" -ne 0 ]
then echo "usage: load_custom_controller <no arguments>"
else rostopic pub -1 /PPSClient/Command std_msgs/Int32 2;
fi
#!/bin/bash
if [ "$#" -ne 0 ]
then echo "usage: load_safe_controller <no arguments>"
else rostopic pub -1 /PPSClient/Command std_msgs/Int32 1;
fi
if [ "$#" -ne 4 ] ;
#!/bin/bash
if [ "$#" -ne 4 ]
then echo "usage: safe_controller_setpoint <x> <y> <z> <yaw>"
else rostopic pub -1 /SafeControllerService/Setpoint d_fall_pps/Setpoint "{x: $0, y: $1, z: $2, yaw: $3}";
fi
......@@ -14,176 +14,181 @@
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//TODO:
//CentralManager: extract data about room from vicon data
//CentralManager: assign localArea for each group and those coordinates to PPSClients
//ViconDataPublisher: extract data about room from vicon data in and send also to PPSClient
//PPSClient: Compare data received from CentralManager and ViconDataPublisher and determine in which localArea you are
//PPSClient: Choose correct controller accoring to current localArea
#include "ros/ros.h"
#include <stdlib.h>
//include autogenerated headers from srv files
#include "d_fall_pps/Controller.h"
#include "d_fall_pps/CentralManager.h"
//include autogenerated headers from msg files
#include "d_fall_pps/ViconData.h"
#include "d_fall_pps/ControlCommand.h"
#include "d_fall_pps/CrazyflieContext.h"
#include "std_msgs/Int32.h"
#include "d_fall_pps/ControlCommand.h"
#define CMD_USE_SAFE_CONTROLLER 1
#define CMD_USE_CUSTOM_CONTROLLER 2
using namespace d_fall_pps;
//the teamname and the assigned crazyflie, will be extracted from studentParams.yaml
std::string team; //is this needed here? maybe for room asignment received from CentralManager?
std::string cflie;
//name of the student team
std::string teamName;
//name of the crazyflie, as specified in Vicon
std::string crazyflieName;
//global sevices
//the safe controller specified in the ClientConfig.yaml, is considered trusted
ros::ServiceClient safeController;
ros::ServiceClient centralClient;
//the custom controller specified in the ClientConfig.yaml, is considered untrusted
ros::ServiceClient customController;
ros::ServiceClient centralManager;
ros::Publisher controlCommandPublisher;
AreaBounds localArea;
void ppsClientToController(ViconData data, bool autocontrolOn){
//call safecontroller if autocontrol is true
if(autocontrolOn){
//TBD: call safecontroller here
//for the moment just switch the motor off
ROS_INFO_STREAM("AutocontrolOn >>>>>> SWITCHED OFF");
ControlCommand switchOffControls;
switchOffControls.roll = 0;
switchOffControls.pitch = 0;
switchOffControls.yaw = 0;
switchOffControls.motorCmd1 = 0;
switchOffControls.motorCmd2 = 0;
switchOffControls.motorCmd3 = 0;
switchOffControls.motorCmd4 = 0;
switchOffControls.onboardControllerType = 0;
controlCommandPublisher.publish(switchOffControls);
}
else {
//student controller is called here
//for the moment use safecontroller for TESTING
Controller srvRate;
srvRate.request.crazyflieLocation = data;
//TODO:
//return control commands
if(safeController.call(srvRate)){
//ROS_INFO("Received control output");
//ROS_INFO_STREAM(srvRate.response.controlOutput);
//check attitude an
controlCommandPublisher.publish(srvRate.response.controlOutput);
} else {
ROS_ERROR("Failed to call SafeControllerService");
//return 1; //return some useful stuff
}
}
}
//describes the area of the crazyflie and other parameters
CrazyflieContext context;
//wheter to use safe of custom controller
bool usingSafeController;
//acceptance test for crazyflie position
bool safetyCheck(ViconData data){
//checks if crazyflie is within allowed area and if custom controller returns no data
bool safetyCheck(ViconData data, ControlCommand controlCommand) {
CrazyflieContext CrazyflieContext;
//position check
if((data.x < localArea.xmin) or (data.x > localArea.xmax)){
if((data.x < context.localArea.xmin) or (data.x > context.localArea.xmax)) {
return true;
}
if((data.y < localArea.ymin) or (data.y > localArea.ymax)){
if((data.y < context.localArea.ymin) or (data.y > context.localArea.ymax)) {
return true;
}
if((data.z < localArea.zmin) or (data.z > localArea.zmax)){
if((data.z < context.localArea.zmin) or (data.z > context.localArea.zmax)) {
return true;
}
//all checks passed
return false;
}
//is called upon every new arrival of data in main
void viconCallback(const ViconData& data){
//ROS_INFO("My teamname is:"); ROS_INFO_STREAM(team);
//ROS_INFO("My crazyflie is:"); ROS_INFO_STREAM(cflie);
if(data.crazyflieName == cflie){
//is called when new data from Vicon arrives
void viconCallback(const ViconData& data) {
if(data.crazyflieName == crazyflieName) {
ROS_INFO_STREAM(data);
//forward data to safety check
bool autocontrolOn = safetyCheck(data);
ppsClientToController(data, autocontrolOn);
Controller controllerCall;
controllerCall.request.crazyflieLocation = data;
if(!usingSafeController) {
bool success = customController.call(controllerCall);
if(!success) {
ROS_ERROR("Failed to call custom controller, switching to safe controller");
usingSafeController = true;
}
//usingSafeController = !safetyCheck(data, controllerCall.response.controlOutput);
usingSafeController = true; //debug
}
if(usingSafeController) {
bool success = safeController.call(controllerCall);
if(!success) {
ROS_ERROR("Failed to call safe controller");
}
}
controlCommandPublisher.publish(controllerCall.response.controlOutput);
} else {
ROS_INFO_STREAM("Ignoring data from other crazyflie '" << data.crazyflieName << "'");
}
}
void loadParameters(ros::NodeHandle& nodeHandle) {
if(!nodeHandle.getParam("teamName", teamName)) {
ROS_ERROR("Failed to get teamName");
}
else {
ROS_INFO_STREAM("ViconData from other crazyflie received: " << data.crazyflieName);
if(!nodeHandle.getParam("crazyFlieName", crazyflieName)) {
ROS_ERROR("Failed to get crazyFlieName");
}
}
int main(int argc, char* argv[]){
ROS_INFO_STREAM("PPSClient started");
void loadCrazyflieContext() {
CentralManager centralManagerCall;
if(centralManager.call(centralManagerCall)) {
context = centralManagerCall.response.context;
ROS_INFO("CrazyflieContext obtained");
} else {
ROS_ERROR("Failed to call CentralManagerService");
}
}
ros::init(argc, argv, "PPSClient");
void loadSafeController() {
ros::NodeHandle nodeHandle("~");
std::string safeControllerName;
if(!nodeHandle.getParam("safeController", safeControllerName)) {
ROS_ERROR("Failed to get safe controller name");
return;
}
ros::service::waitForService(safeControllerName);
safeController = nodeHandle.serviceClient<Controller>(safeControllerName, true);
ROS_INFO_STREAM("loaded safe controller " << safeControllerName);
}
void loadCustomController() {
ros::NodeHandle nodeHandle("~");
//get the params defined in ClientConfig.yaml
if(!nodeHandle.getParam("TeamName",team)){
ROS_ERROR("Failed to get TeamName");
std::string customControllerName;
if(!nodeHandle.getParam("customController", customControllerName)) {
ROS_ERROR("Failed to get custom controller name");
return;
}
if(!nodeHandle.getParam("CrazyFlieName",cflie)){
ROS_ERROR("Failed to get CrazyFlieName");
customController = nodeHandle.serviceClient<Controller>(customControllerName, true);
ROS_INFO_STREAM("loaded custom controller " << customControllerName);
}
void commandCallback(const std_msgs::Int32& commandMsg) {
int cmd = commandMsg.data;
switch(cmd) {
case CMD_USE_SAFE_CONTROLLER:
loadSafeController();
usingSafeController = true;
break;
case CMD_USE_CUSTOM_CONTROLLER:
loadCustomController();
usingSafeController = false;
break;
default:
ROS_ERROR_STREAM("unexpected command number: " << cmd);
break;
}
}
int main(int argc, char* argv[]){
ros::init(argc, argv, "PPSClient");
ros::NodeHandle nodeHandle("~");
loadParameters(nodeHandle);
ros::Subscriber viconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 1, viconCallback);
//keeps 100 messages because otherwise ViconDataPublisher would override the data immediately
ros::Subscriber viconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 100, viconCallback);
ROS_INFO_STREAM("successfully subscribed to ViconData");
//ros::Publishers to advertise the control output
controlCommandPublisher = nodeHandle.advertise <ControlCommand>("ControlCommand", 1);
//this topic lets the PPSClient listen to the terminal commands
ros::Publisher commandPublisher = nodeHandle.advertise<std_msgs::Int32>("Command", 1);
ros::Subscriber commandSubscriber = nodeHandle.subscribe("/PPSClient/Command", 1, commandCallback);
//service
//to be expanded with additional services depending on controller (currently only one available)
ros::service::waitForService("/SafeControllerService/RateController");
safeController = nodeHandle.serviceClient<Controller>("/SafeControllerService/RateController", true);
//safeController = 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<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)){
localArea = ManagerSettings.response.context.localArea;
ROS_INFO("CentralManager responded");
ROS_INFO("localAreaBoundaries Set");
//start with safe controller
usingSafeController = true;
loadSafeController();
}
else{
ROS_ERROR("Failed to call CentralManagerService. Callback is aborted");
//return some useful stuff
return 0;
}
//<<<<<<<<<<<<<<<<<<<<<<<this is only for testing
//ros::service::waitForService("/CentralManagerService/CentralManager");
centralManager = nodeHandle.serviceClient<CentralManager>("/CentralManagerService/CentralManager");
loadCrazyflieContext();
ros::spin();
return 0;
......
......@@ -275,12 +275,9 @@ int main(int argc, char* argv[]) {
ros::NodeHandle nodeHandle("~");
loadParameters(nodeHandle);
ROS_INFO("<aaaaaaaa");
ros::Publisher setpointPublisher = nodeHandle.advertise<Setpoint>("Setpoint", 1);
ros::Subscriber setpointSubscriber = nodeHandle.subscribe("/SafeControllerService/Setpoint", 1, setpointCallback);
ROS_INFO("<aaaaaaabbbbbbbb");
ros::ServiceServer service = nodeHandle.advertiseService("RateController", calculateControlOutput);
ROS_INFO("SafeControllerService ready");
ros::spin();
......
......@@ -34,8 +34,6 @@
using namespace ViconDataStreamSDK::CPP;
using namespace d_fall_pps;
std::string cflie;
int main(int argc, char* argv[]) {
ros::init(argc, argv, "ViconDataPublisher");
......@@ -44,9 +42,6 @@ int main(int argc, char* argv[]) {
//get Crazyflie Name from Params, so we can filter the Vicon data by the name
if(!nodeHandle.getParam("CrazyFlieName",cflie)){
ROS_ERROR("Failed to get CrazyFlieName");
}
ros::Publisher viconDataPublisher =
nodeHandle.advertise<ViconData>("ViconData", 1);
......@@ -125,12 +120,6 @@ int main(int argc, char* argv[]) {
//continue only if the received frame is for the correct crazyflie
if(subjectName != cflie){
//no publishing needed
ROS_INFO_STREAM("Not publishing Vicon because data is from flie: " << subjectName);
} else {
Output_GetSegmentGlobalTranslation outputTranslation =
client.GetSegmentGlobalTranslation(subjectName, segmentName);
//ROS_INFO_STREAM("translation occluded: " << outputTranslation.Occluded);
......@@ -139,7 +128,6 @@ int main(int argc, char* argv[]) {
client.GetSegmentGlobalRotationQuaternion(subjectName, segmentName);
//ROS_INFO_STREAM("translation occluded: " << outputRotation.Occluded);
//calculate position and rotation of Crazyflie
double quat_x = outputRotation.Rotation[0];
double quat_y = outputRotation.Rotation[1];
......@@ -174,7 +162,6 @@ int main(int argc, char* argv[]) {
//finally publish
viconDataPublisher.publish(viconData);
}
}
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment