Commit c23733a8 authored by bucyril's avatar bucyril
Browse files

supposedly fixed problem caused when multiple Cf-Objects are selected in...

supposedly fixed problem caused when multiple Cf-Objects are selected in Vicon. Plus some cosmetic changes
parent 473ab35b
......@@ -8,6 +8,7 @@
</node>
<node pkg="d_fall_pps" name="ViconDataPublisher" output="screen" type="ViconDataPublisher">
<rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" />
</node>
<node pkg="d_fall_pps" name="PPSClient" output="screen" type="PPSClient">
......
TeamName: 'Two'
CrazyFlieName: "cfFour"
CrazyFlieAddress: "radio://0/99/2M"
CrazyFlieName: "cfThree"
CrazyFlieAddress: "radio://0/72/2M"
SafeController: ""
SafeControllerType: ""
CustomController: ""
......
......@@ -85,9 +85,7 @@ void ppsClientToController(ViconData data, bool autocontrolOn){
//check attitude an
controlCommandPublisher.publish(srvRate.response.controlOutput);
//onboardControllerType = ??????????????????????
controlCommandPublisher.publish(srvRate.response.controlOutput);
} else {
ROS_ERROR("Failed to call SafeControllerService");
......@@ -128,7 +126,7 @@ void viconCallback(const ViconData& data){
ppsClientToController(data, autocontrolOn);
}
else {
ROS_INFO("ViconData from other crazyflie received");
ROS_INFO_STREAM("ViconData from other crazyflie received: " << data.crazyflieName);
}
......@@ -140,7 +138,7 @@ int main(int argc, char* argv[]){
ros::init(argc, argv, "PPSClient");
ros::NodeHandle nodeHandle("~");
//get the params defined in studentParams.yaml
//get the params defined in ClientConfig.yaml
if(!nodeHandle.getParam("TeamName",team)){
ROS_ERROR("Failed to get TeamName");
}
......
......@@ -81,6 +81,7 @@ float computeMotorPolyBackward(float thrust) {
return (-motorPoly[1] + sqrt(motorPoly[1] * motorPoly[1] - 4 * motorPoly[2] * (motorPoly[0] - thrust))) / (2 * motorPoly[2]);
}
//Kalman
void estimateState(Controller::Request &request, float (&est)[9]) {
// attitude
est[6] = request.crazyflieLocation.roll;
......@@ -94,6 +95,7 @@ void estimateState(Controller::Request &request, float (&est)[9]) {
ahat_x[4] = estimatorMatrix[0] * prevEstimate[1] + estimatorMatrix[1] * prevEstimate[4];
ahat_x[5] = estimatorMatrix[0] * prevEstimate[2] + estimatorMatrix[1] * prevEstimate[5];
ROS_INFO_STREAM("est prevEstimate[0]: " << prevEstimate[0]);
ROS_INFO_STREAM("est prevEstimate[3]: " << prevEstimate[3]);
ROS_INFO_STREAM("est prevEstimate[1]: " << prevEstimate[1]);
......@@ -104,6 +106,7 @@ void estimateState(Controller::Request &request, float (&est)[9]) {
ROS_INFO_STREAM("est request.crazyflieLocation.x: " << request.crazyflieLocation.x);
ROS_INFO_STREAM("est request.crazyflieLocation.y: " << request.crazyflieLocation.y);
ROS_INFO_STREAM("est request.crazyflieLocation.z: " << request.crazyflieLocation.z);
float k_x[6]; //filterGain times state
k_x[0] = request.crazyflieLocation.x * filterGain[0];
......@@ -113,6 +116,7 @@ void estimateState(Controller::Request &request, float (&est)[9]) {
k_x[4] = request.crazyflieLocation.y * filterGain[4];
k_x[5] = request.crazyflieLocation.z * filterGain[5];
ROS_INFO_STREAM("est k_x x: " << k_x[0]);
ROS_INFO_STREAM("est k_x y: " << k_x[1]);
ROS_INFO_STREAM("est k_x z: " << k_x[2]);
......@@ -126,6 +130,7 @@ void estimateState(Controller::Request &request, float (&est)[9]) {
ROS_INFO_STREAM("est ahat_x vx: " << ahat_x[3]);
ROS_INFO_STREAM("est ahat_x vy: " << ahat_x[4]);
ROS_INFO_STREAM("est ahat_x vz: " << ahat_x[5]);
est[0] = ahat_x[0] + k_x[0];
est[1] = ahat_x[1] + k_x[1];
......@@ -136,6 +141,7 @@ void estimateState(Controller::Request &request, float (&est)[9]) {
memcpy(prevEstimate, est, 9 * sizeof(float));
ROS_INFO_STREAM("est x: " << est[0]);
ROS_INFO_STREAM("est y: " << est[1]);
ROS_INFO_STREAM("est z: " << est[2]);
......@@ -147,8 +153,10 @@ void estimateState(Controller::Request &request, float (&est)[9]) {
ROS_INFO_STREAM("est r: " << est[6]);
ROS_INFO_STREAM("est p: " << est[7]);
ROS_INFO_STREAM("est y: " << est[8]);
}
//simple derivative
/*void estimateState(Controller::Request &request, float (&est)[9]) {
est[0] = request.crazyflieLocation.x;
est[1] = request.crazyflieLocation.y;
......@@ -217,7 +225,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
//HINWEIS: übersteuern beim outYaw wenn man 180 Grad zum yaw-Setpoint startet
//nach Multiplikation mit 0.5 gibt es den Effekt nicht mehr -> mit Paul besprechen....
//outYaw = outYaw * 0.5;
outYaw *= 0.5;
response.controlOutput.roll = outRoll;
response.controlOutput.pitch = outPitch;
......
......@@ -34,12 +34,20 @@
using namespace ViconDataStreamSDK::CPP;
using namespace d_fall_pps;
std::string cflie;
int main(int argc, char* argv[]) {
ros::init(argc, argv, "ViconDataPublisher");
ros::NodeHandle nodeHandle("~");
ros::Time::init();
//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);
......@@ -95,7 +103,7 @@ int main(int argc, char* argv[]) {
//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);
ROS_INFO("iteration #%d of ViconDataPublisher",iterations);
}
iterations++;
......@@ -116,6 +124,13 @@ int main(int argc, char* argv[]) {
std::string segmentName = client.GetSegmentName(subjectName, 0).SegmentName; //last value had to be changed to 0 instead of index, 27.03.17
//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);
......@@ -145,7 +160,7 @@ int main(int argc, char* argv[]) {
totalViconLatency = 0;
}
//build and publish message
//build message
ViconData viconData;
viconData.crazyflieName = subjectName;
......@@ -159,6 +174,7 @@ 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