Commit e6b08425 authored by mastefan's avatar mastefan
Browse files

mothership2 added, param tuning

parent 64b81823
...@@ -270,6 +270,8 @@ float xm1_x_distance; ...@@ -270,6 +270,8 @@ float xm1_x_distance;
float xm2_distance_to_ms; float xm2_distance_to_ms;
float xm2_height_over_ms; float xm2_height_over_ms;
float xm1_height;
float xm1_normalizing_factor = 3; // normalize the difference between xcf0 and xms float xm1_normalizing_factor = 3; // normalize the difference between xcf0 and xms
float xm1_scaling_factor = 0.6; // Scale in the direction of the the difference between xcf0 and xms float xm1_scaling_factor = 0.6; // Scale in the direction of the the difference between xcf0 and xms
...@@ -286,6 +288,7 @@ float trajectory_duration = 0; ...@@ -286,6 +288,7 @@ float trajectory_duration = 0;
float trajectory_setpoint[4]; // x,y,z,yaw float trajectory_setpoint[4]; // x,y,z,yaw
float trajectory_velocity[3]; // x,y,z float trajectory_velocity[3]; // x,y,z
// Origin of the Flying zone // Origin of the Flying zone
float originX = 0; float originX = 0;
float originY = 0; float originY = 0;
......
...@@ -30,8 +30,11 @@ xm1_x_distance: 0.5 ...@@ -30,8 +30,11 @@ xm1_x_distance: 0.5
# distance from MS to second point on trajectory # distance from MS to second point on trajectory
xm2_distance_to_ms: 0.6 xm2_distance_to_ms: 0.6
xm2_height_over_ms: 0.3 xm2_height_over_ms: 0.3
xm1_height: 0.6
# velocity of trajctory # velocity of trajctory
trajectory_velocity_of_CF : 2.0 trajectory_velocity_of_CF : 3.0
xm1_normalizing_factor : 3 # normalize the difference between xcf0 and xms xm1_normalizing_factor : 3 # normalize the difference between xcf0 and xms
xm1_scaling_factor : 0.6 # Scale in the direction of the the difference between xcf0 and xms xm1_scaling_factor : 0.6 # Scale in the direction of the the difference between xcf0 and xms
...@@ -41,7 +44,7 @@ xm2_distance_to_ms_scaling_factor : 1 # Scale in direction of the mothersh ...@@ -41,7 +44,7 @@ xm2_distance_to_ms_scaling_factor : 1 # Scale in direction of the mothersh
# tolerances # tolerances
tol_takeoff: [0.07, 0.07, 0.07] tol_takeoff: [0.07, 0.07, 0.07]
tol_approach: [0.2, 0.2, 0.1] tol_approach: [0.2, 0.2, 0.1]
tol_land: [0.07, 0.07, 0.07] tol_land: [0.07, 0.07, 0.04]
......
...@@ -925,7 +925,7 @@ std::vector<float> trajectory_to_ms(Controller::Request &request, double t){ ...@@ -925,7 +925,7 @@ std::vector<float> trajectory_to_ms(Controller::Request &request, double t){
xm1[0] = xcf0[0] + xm1_x_distance; // xm1_x_distance hardcoded at the moment xm1[0] = xcf0[0] + xm1_x_distance; // xm1_x_distance hardcoded at the moment
xm1[1] = xcf0[1]; xm1[1] = xcf0[1];
xm1[2] = request.otherCrazyflies[0].z+0.6; xm1[2] = request.otherCrazyflies[0].z + xm1_height;
first_trajectory_calculation = false; first_trajectory_calculation = false;
} }
...@@ -1536,26 +1536,34 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle ...@@ -1536,26 +1536,34 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle
// Publish the "debugMsg" // Publish the "debugMsg"
debugPublisher.publish(debugMsg); debugPublisher.publish(debugMsg);
float ms_sinYaw = sin(0.5*request.otherCrazyflies[0].yaw);
float ms_cosYaw = cos(0.5*request.otherCrazyflies[0].yaw);
float ms_sinRoll = sin(0.5*request.otherCrazyflies[0].roll);
float ms_cosRoll = cos(0.5*request.otherCrazyflies[0].roll);
float ms_sinPitch = sin(0.5*request.otherCrazyflies[0].pitch);
float ms_cosPitch = cos(0.5*request.otherCrazyflies[0].pitch);
visualization_msgs::Marker rviz_mothership; visualization_msgs::Marker rviz_mothership;
rviz_mothership.pose.position.x = request.otherCrazyflies[0].x; rviz_mothership.pose.position.x = request.otherCrazyflies[0].x;
rviz_mothership.pose.position.y = request.otherCrazyflies[0].y; rviz_mothership.pose.position.y = request.otherCrazyflies[0].y;
rviz_mothership.pose.position.z = request.otherCrazyflies[0].z; rviz_mothership.pose.position.z = request.otherCrazyflies[0].z-0.05;
rviz_mothership.header.frame_id = "/frame_Trajectory"; rviz_mothership.header.frame_id = "/frame_Trajectory";
rviz_mothership.header.stamp = ros::Time::now(); rviz_mothership.header.stamp = ros::Time::now();
rviz_mothership.ns = "mothership"; rviz_mothership.ns = "mothership";
rviz_mothership.action = visualization_msgs::Marker::ADD; rviz_mothership.action = visualization_msgs::Marker::ADD;
rviz_mothership.pose.orientation.z = request.otherCrazyflies[0].yaw; rviz_mothership.pose.orientation.w = ms_cosRoll * ms_cosPitch * ms_cosYaw + ms_sinRoll * ms_sinPitch * ms_sinYaw;
rviz_mothership.pose.orientation.w = 1.0;//request.otherCrazyflies[0].yaw; rviz_mothership.pose.orientation.x = ms_sinRoll * ms_cosPitch * ms_cosYaw - ms_cosRoll * ms_sinPitch * ms_sinYaw;
rviz_mothership.pose.orientation.y = ms_cosRoll * ms_sinPitch * ms_cosYaw + ms_sinRoll * ms_cosPitch * ms_sinYaw;
rviz_mothership.pose.orientation.z = ms_cosRoll * ms_cosPitch * ms_sinYaw - ms_sinRoll * ms_sinPitch * ms_cosYaw;
rviz_mothership.id = 0; rviz_mothership.id = 0;
rviz_mothership.type = visualization_msgs::Marker::CUBE; rviz_mothership.type = visualization_msgs::Marker::CUBE;
rviz_mothership.scale.x = 0.4; rviz_mothership.scale.x = 0.58f;
rviz_mothership.scale.y = 0.25; rviz_mothership.scale.y = 0.30f;
rviz_mothership.scale.z = 0.01; rviz_mothership.scale.z = 0.01f;
rviz_mothership.color.r = 0.0f; rviz_mothership.color.r = 0.0f;
rviz_mothership.color.g = 0.0f; rviz_mothership.color.g = 0.0f;
...@@ -2113,6 +2121,8 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle) ...@@ -2113,6 +2121,8 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
xm1_x_distance = getParameterFloat(nodeHandle_for_dronexController, "xm1_x_distance"); xm1_x_distance = getParameterFloat(nodeHandle_for_dronexController, "xm1_x_distance");
xm2_distance_to_ms = getParameterFloat(nodeHandle_for_dronexController, "xm2_distance_to_ms"); xm2_distance_to_ms = getParameterFloat(nodeHandle_for_dronexController, "xm2_distance_to_ms");
xm2_height_over_ms = getParameterFloat(nodeHandle_for_dronexController, "xm2_height_over_ms"); xm2_height_over_ms = getParameterFloat(nodeHandle_for_dronexController, "xm2_height_over_ms");
xm1_height = getParameterFloat(nodeHandle_for_dronexController, "xm2_height_over_ms");
trajectory_velocity_of_CF = getParameterFloat(nodeHandle_for_dronexController, "trajectory_velocity_of_CF"); trajectory_velocity_of_CF = getParameterFloat(nodeHandle_for_dronexController, "trajectory_velocity_of_CF");
xm1_normalizing_factor = getParameterFloat(nodeHandle_for_dronexController, "xm1_normalizing_factor"); xm1_normalizing_factor = getParameterFloat(nodeHandle_for_dronexController, "xm1_normalizing_factor");
......
...@@ -209,7 +209,7 @@ void goToControllerSetpoint() ...@@ -209,7 +209,7 @@ void goToControllerSetpoint()
void viconCallback(const ViconData& viconData) void viconCallback(const ViconData& viconData)
{ {
CrazyflieData droneX; CrazyflieData droneX, droneX2;
for(std::vector<CrazyflieData>::const_iterator it = viconData.crazyflies.begin(); it != viconData.crazyflies.end(); ++it) { for(std::vector<CrazyflieData>::const_iterator it = viconData.crazyflies.begin(); it != viconData.crazyflies.end(); ++it) {
CrazyflieData global = *it; CrazyflieData global = *it;
...@@ -218,6 +218,10 @@ void viconCallback(const ViconData& viconData) ...@@ -218,6 +218,10 @@ void viconCallback(const ViconData& viconData)
droneX = global; droneX = global;
coordinatesToLocal(droneX); coordinatesToLocal(droneX);
} }
if(global.crazyflieName == "Mothership2"){
droneX2 = global;
coordinatesToLocal(droneX2);
}
} }
...@@ -234,6 +238,7 @@ void viconCallback(const ViconData& viconData) ...@@ -234,6 +238,7 @@ void viconCallback(const ViconData& viconData)
coordinatesToLocal(local); coordinatesToLocal(local);
controllerCall.request.ownCrazyflie = local; controllerCall.request.ownCrazyflie = local;
controllerCall.request.otherCrazyflies.push_back(droneX); controllerCall.request.otherCrazyflies.push_back(droneX);
controllerCall.request.otherCrazyflies.push_back(droneX2);
ros::NodeHandle nodeHandle("~"); ros::NodeHandle nodeHandle("~");
......
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