Commit 8eb7eb0b authored by https://roangel@gitlab.ethz.ch's avatar https://roangel@gitlab.ethz.ch
Browse files

changed ViconDataPublisher and messages as discussed this morning

parent 2fe0ab3f
......@@ -96,7 +96,7 @@ qt5_wrap_cpp(SRC_MOC_HDRS ${SRC_HDRS_QOBJECT})
add_message_files(
FILES
UnlabeledMarker.msg
UnlabeledMarkersArray.msg
CrazyflieData.msg
ViconData.msg
ControlCommand.msg
CrazyflieContext.msg
......
string crazyflieName
float64 x
float64 y
float64 z
float64 roll
float64 pitch
float64 yaw
float64 acquiringTime #delta t
UnlabeledMarker[] markers
\ No newline at end of file
string crazyflieName
float64 x
float64 y
float64 z
float64 roll
float64 pitch
float64 yaw
float64 acquiringTime #delta t
CrazyflieData[] crazyflies
UnlabeledMarker[] markers
......@@ -61,13 +61,13 @@ void convertIntoBodyFrame(float est[9], float (&estBody)[9], int yaw_measured) {
bool calculateControlOutput(Controller::Request &request, Controller::Response &response) {
//writing the data from -request- to command line
//might be useful for debugging
ROS_INFO_STREAM("x-coordinates: " << request.crazyflieLocation.x);
ROS_INFO_STREAM("y-coordinates: " << request.crazyflieLocation.y);
ROS_INFO_STREAM("z-coordinates: " << request.crazyflieLocation.z);
ROS_INFO_STREAM("roll: " << request.crazyflieLocation.roll);
ROS_INFO_STREAM("pitch: " << request.crazyflieLocation.pitch);
ROS_INFO_STREAM("yaw: " << request.crazyflieLocation.yaw);
ROS_INFO_STREAM("Delta t: " << request.crazyflieLocation.acquiringTime);
ROS_INFO_STREAM("x-coordinates: " << request.ownCrazyflie.x);
ROS_INFO_STREAM("y-coordinates: " << request.ownCrazyflie.y);
ROS_INFO_STREAM("z-coordinates: " << request.ownCrazyflie.z);
ROS_INFO_STREAM("roll: " << request.ownCrazyflie.roll);
ROS_INFO_STREAM("pitch: " << request.ownCrazyflie.pitch);
ROS_INFO_STREAM("yaw: " << request.ownCrazyflie.yaw);
ROS_INFO_STREAM("Delta t: " << request.ownCrazyflie.acquiringTime);
......
......@@ -23,6 +23,7 @@
#include "d_fall_pps/CentralManager.h"
#include "d_fall_pps/ViconData.h"
#include "d_fall_pps/CrazyflieData.h"
#include "d_fall_pps/ControlCommand.h"
#include "d_fall_pps/CrazyflieContext.h"
#include "std_msgs/Int32.h"
......@@ -61,7 +62,7 @@ bool crazyflieEnabled;
int safetyDelay;
//checks if crazyflie is within allowed area and if custom controller returns no data
bool safetyCheck(ViconData data, ControlCommand controlCommand) {
bool safetyCheck(CrazyflieData data, ControlCommand controlCommand) {
//position check
if((data.x < context.localArea.xmin) or (data.x > context.localArea.xmax)) {
......@@ -82,75 +83,78 @@ bool safetyCheck(ViconData data, ControlCommand controlCommand) {
}
//is called when new data from Vicon arrives
void viconCallback(const ViconData& data) {
if(data.crazyflieName == crazyflieName) {
Controller controllerCall;
controllerCall.request.crazyflieLocation = data;
if(crazyflieEnabled){
if(!usingSafeController) {
bool success = customController.call(controllerCall);
if(!success) {
ROS_ERROR("Failed to call custom controller, switching to safe controller");
usingSafeController = true;
}
void viconCallback(const ViconData& viconData) {
for(std::vector<CrazyflieData>::const_iterator it = viconData.crazyflies.begin(); it != viconData.crazyflies.end(); ++it) {
CrazyflieData data = *it;
if(data.crazyflieName == crazyflieName) {
Controller controllerCall;
controllerCall.request.ownCrazyflie = data;
if(crazyflieEnabled){
if(!usingSafeController) {
bool success = customController.call(controllerCall);
if(!success) {
ROS_ERROR("Failed to call custom controller, switching to safe controller");
usingSafeController = true;
}
usingSafeController = true; //debug
}
if(usingSafeController) {
bool success = safeController.call(controllerCall);
if(!success) {
ROS_ERROR("Failed to call safe controller");
usingSafeController = true; //debug
}
}
/*
if(!safetyCheck(data, controllerCall.response.controlOutput)){
ROS_INFO_STREAM("AutocontrolOn >>>>>> SWITCHED OFF");
if(safetyDelay == 0){
ROS_INFO_STREAM("ROS Shutdown");
//bag.close();
ros::shutdown();
}
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;
controllerCall.response.controlOutput = switchOffControls;
}
else{
safetyDelay=20;
}
if(usingSafeController) {
bool success = safeController.call(controllerCall);
if(!success) {
ROS_ERROR("Failed to call safe controller");
}
}
controlCommandPublisher.publish(controllerCall.response.controlOutput);
std_msgs::String str;
str.data = std::string("foo");
std_msgs::Int32 i;
i.data = 42;
bag.write("testfoo: ", ros::Time::now(), str);
bag.write("test42: ", ros::Time::now(), i);
*/
controlCommandPublisher.publish(controllerCall.response.controlOutput);
} else{ //crazyflie disabled
ControlCommand zeroOutput = ControlCommand(); //everything set to zero
zeroOutput.onboardControllerType = 2; //set to motor_mode
controlCommandPublisher.publish(zeroOutput);
/*
if(!safetyCheck(data, controllerCall.response.controlOutput)){
ROS_INFO_STREAM("AutocontrolOn >>>>>> SWITCHED OFF");
if(safetyDelay == 0){
ROS_INFO_STREAM("ROS Shutdown");
//bag.close();
ros::shutdown();
}
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;
controllerCall.response.controlOutput = switchOffControls;
}
else{
safetyDelay=20;
}
controlCommandPublisher.publish(controllerCall.response.controlOutput);
std_msgs::String str;
str.data = std::string("foo");
std_msgs::Int32 i;
i.data = 42;
bag.write("testfoo: ", ros::Time::now(), str);
bag.write("test42: ", ros::Time::now(), i);
*/
controlCommandPublisher.publish(controllerCall.response.controlOutput);
} else{ //crazyflie disabled
ControlCommand zeroOutput = ControlCommand(); //everything set to zero
zeroOutput.onboardControllerType = 2; //set to motor_mode
controlCommandPublisher.publish(zeroOutput);
}
}
}
}
......
......@@ -17,7 +17,7 @@
#include <math.h>
#include <stdlib.h>
#include "ros/ros.h"
#include "d_fall_pps/ViconData.h"
#include "d_fall_pps/CrazyflieData.h"
#include "d_fall_pps/Setpoint.h"
#include "d_fall_pps/ControlCommand.h"
#include "d_fall_pps/Controller.h"
......@@ -46,7 +46,7 @@ float prevEstimate[9];
std::vector<float> setpoint(4);
float saturationThrust;
ViconData previousLocation;
CrazyflieData previousLocation;
void loadParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length) {
if(!nodeHandle.getParam(name, val)){
......@@ -85,9 +85,9 @@ float computeMotorPolyBackward(float thrust) {
//Kalman
void estimateState(Controller::Request &request, float (&est)[9]) {
// attitude
est[6] = request.crazyflieLocation.roll;
est[7] = request.crazyflieLocation.pitch;
est[8] = request.crazyflieLocation.yaw;
est[6] = request.ownCrazyflie.roll;
est[7] = request.ownCrazyflie.pitch;
est[8] = request.ownCrazyflie.yaw;
//velocity & filtering
float ahat_x[6]; //estimator matrix times state (x, y, z, vx, vy, vz)
......@@ -98,12 +98,12 @@ void estimateState(Controller::Request &request, float (&est)[9]) {
float k_x[6]; //filterGain times state
k_x[0] = request.crazyflieLocation.x * filterGain[0];
k_x[1] = request.crazyflieLocation.y * filterGain[1];
k_x[2] = request.crazyflieLocation.z * filterGain[2];
k_x[3] = request.crazyflieLocation.x * filterGain[3];
k_x[4] = request.crazyflieLocation.y * filterGain[4];
k_x[5] = request.crazyflieLocation.z * filterGain[5];
k_x[0] = request.ownCrazyflie.x * filterGain[0];
k_x[1] = request.ownCrazyflie.y * filterGain[1];
k_x[2] = request.ownCrazyflie.z * filterGain[2];
k_x[3] = request.ownCrazyflie.x * filterGain[3];
k_x[4] = request.ownCrazyflie.y * filterGain[4];
k_x[5] = request.ownCrazyflie.z * filterGain[5];
est[0] = ahat_x[0] + k_x[0];
est[1] = ahat_x[1] + k_x[1];
......@@ -120,21 +120,21 @@ void estimateState(Controller::Request &request, float (&est)[9]) {
//simple derivative
/*
void estimateState(Controller::Request &request, float (&est)[9]) {
est[0] = request.crazyflieLocation.x;
est[1] = request.crazyflieLocation.y;
est[2] = request.crazyflieLocation.z;
est[0] = request.ownCrazyflie.x;
est[1] = request.ownCrazyflie.y;
est[2] = request.ownCrazyflie.z;
est[3] = (request.crazyflieLocation.x - previousLocation.x) / request.crazyflieLocation.acquiringTime;
est[4] = (request.crazyflieLocation.y - previousLocation.y) / request.crazyflieLocation.acquiringTime;
est[5] = (request.crazyflieLocation.z - previousLocation.z) / request.crazyflieLocation.acquiringTime;
est[3] = (request.ownCrazyflie.x - previousLocation.x) / request.ownCrazyflie.acquiringTime;
est[4] = (request.ownCrazyflie.y - previousLocation.y) / request.ownCrazyflie.acquiringTime;
est[5] = (request.ownCrazyflie.z - previousLocation.z) / request.ownCrazyflie.acquiringTime;
ROS_INFO_STREAM("vx: " << est[3]);
ROS_INFO_STREAM("vy: " << est[4]);
ROS_INFO_STREAM("vz: " << est[5]);
est[6] = request.crazyflieLocation.roll;
est[7] = request.crazyflieLocation.pitch;
est[8] = request.crazyflieLocation.yaw;
est[6] = request.ownCrazyflie.roll;
est[7] = request.ownCrazyflie.pitch;
est[8] = request.ownCrazyflie.yaw;
}
*/
......@@ -156,21 +156,21 @@ void convertIntoBodyFrame(float est[9], float (&state)[9], float yaw_measured) {
}
bool calculateControlOutput(Controller::Request &request, Controller::Response &response) {
ViconData vicon = request.crazyflieLocation;
CrazyflieData vicon = request.ownCrazyflie;
//trial>>>>>>>
int yaw_measured = request.crazyflieLocation.yaw;
int yaw_measured = request.ownCrazyflie.yaw;
//<<<<<<
//move coordinate system to make setpoint origin
request.crazyflieLocation.x -= setpoint[0];
request.crazyflieLocation.y -= setpoint[1];
request.crazyflieLocation.z -= setpoint[2];
float yaw = request.crazyflieLocation.yaw - setpoint[3];
request.ownCrazyflie.x -= setpoint[0];
request.ownCrazyflie.y -= setpoint[1];
request.ownCrazyflie.z -= setpoint[2];
float yaw = request.ownCrazyflie.yaw - setpoint[3];
while(yaw > PI) yaw -= 2 * PI;
while(yaw < -PI) yaw += 2 * PI;
request.crazyflieLocation.yaw = yaw;
request.ownCrazyflie.yaw = yaw;
float est[9]; //px, py, pz, vx, vy, vz, roll, pitch, yaw
estimateState(request, est);
......@@ -211,7 +211,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
response.controlOutput.onboardControllerType = RATE_CONTROLLER;
previousLocation = request.crazyflieLocation;
previousLocation = request.ownCrazyflie;
return true;
}
......
......@@ -19,7 +19,6 @@
#include "ros/ros.h"
#include "d_fall_pps/ViconData.h"
#include "d_fall_pps/UnlabeledMarker.h"
#include "d_fall_pps/UnlabeledMarkersArray.h"
// #define TESTING_FAKE_DATA
......@@ -36,9 +35,6 @@ int main(int argc, char* argv[]) {
ros::Publisher viconDataPublisher =
nodeHandle.advertise<ViconData>("ViconData", 1);
ros::Publisher unlabeledMarkersPublisher =
nodeHandle.advertise<UnlabeledMarkersArray>("UnlabeledMarkersArray", 1);
#ifdef TESTING_FAKE_DATA
// Test faking data part
float f = 0;
......@@ -84,9 +80,11 @@ int main(int argc, char* argv[]) {
i++;
}
#else
Client client;
std::string hostName = "10.42.00.15:801";
std::string hostName;
if(!nodeHandle.getParam("hostName", hostName)) {
ROS_ERROR("Failed to get hostName");
return 1;
......@@ -113,33 +111,21 @@ int main(int argc, char* argv[]) {
client.EnableDeviceData();
// Set the global up axis, such that Z is up
client.SetAxisMapping(Direction::Forward,
Direction::Left,
Direction::Up);
client.SetAxisMapping(Direction::Forward, Direction::Left, Direction::Up);
int iterations = 0;
while (ros::ok()) {
//if you want to see at least some output in the terminal
//to see that you are still publishing
if(iterations % 1000 == 0){
ROS_INFO("iteration #%d",iterations);
}
iterations++;
// Output_GetUnlabeledMarkerCount GetUnlabeledMarkerCount() const;
// Get a frame
while (client.GetFrame().Result != Result::Success) {
// Sleep a little so that we don't lumber the CPU with a busy poll
ros::Duration(0.001).sleep();
}
ViconData viconData;
// Unlabeled markers, for GUI
unsigned int unlabeledMarkerCount = client.GetUnlabeledMarkerCount().MarkerCount;
UnlabeledMarker marker;
UnlabeledMarkersArray markersArray;
ROS_INFO_STREAM("unlabeledMarkerCount: " << unlabeledMarkerCount);
for(int unlabeledMarkerIndex = 0; unlabeledMarkerIndex < unlabeledMarkerCount; unlabeledMarkerIndex++)
......@@ -153,11 +139,9 @@ int main(int argc, char* argv[]) {
marker.y = OutputTranslation.Translation[1];
marker.z = OutputTranslation.Translation[2];
markersArray.markers.push_back(marker);
viconData.markers.push_back(marker);
}
unlabeledMarkersPublisher.publish(markersArray);
unsigned int subjectCount = client.GetSubjectCount().SubjectCount;
// //Process the data and publish on topic
......@@ -196,22 +180,20 @@ int main(int argc, char* argv[]) {
}
//build message
ViconData viconData;
viconData.crazyflieName = subjectName;
viconData.x = outputTranslation.Translation[0] / 1000.0f;
viconData.y = outputTranslation.Translation[1] / 1000.0f;
viconData.z = outputTranslation.Translation[2] / 1000.0f;
viconData.roll = roll;
viconData.pitch = pitch;
viconData.yaw = yaw;
viconData.acquiringTime = totalViconLatency;
//finally publish
viconDataPublisher.publish(viconData);
CrazyflieData cfData;
cfData.crazyflieName = subjectName;
cfData.x = outputTranslation.Translation[0] / 1000.0f;
cfData.y = outputTranslation.Translation[1] / 1000.0f;
cfData.z = outputTranslation.Translation[2] / 1000.0f;
cfData.roll = roll;
cfData.pitch = pitch;
cfData.yaw = yaw;
cfData.acquiringTime = totalViconLatency;
viconData.crazyflies.push_back(cfData);
}
viconDataPublisher.publish(viconData);
}
client.DisableSegmentData();
......
ViconData crazyflieLocation
CrazyflieData ownCrazyflie
CrazyflieData[] otherCrazyflies
---
ControlCommand controlOutput
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