Commit e6ed9ee6 authored by muelmarc's avatar muelmarc
Browse files

added area and controlleroutput structs to PPSClient for testing; added...

added area and controlleroutput structs to PPSClient for testing; added CentralManagerService to launch file
parent 88d236aa
......@@ -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):
......
<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" />
......
TeamName: 'Two'
CrazyFlieName: "cfTwo"
CrazyFlieName: "cfFour"
CrazyFlieAddress: "radio://0/99/2M"
SafeController: ""
SafeControllerType: ""
......
......@@ -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;
}
......
......@@ -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;
......
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