Commit 9272a68c authored by mastefan's avatar mastefan
Browse files

VM: xm2 on all sides

parent aa97f2c2
......@@ -240,17 +240,6 @@ std::vector<float> tol_land (3, 0.0);
//TODO we might want to use float64 instead of float
// trajectory generation variables
double trajectory_start_time = 0;
double total_time_since_start = 0;
bool first_trajectory_calculation = true;
// lookahead for trajectory: fetched from .yaml
float trajectory_deltaT_position = 0;
float trajectory_deltaT_velocity = 0;
// Trajectory: xcf0 -> xm1 -> xm2 -> xms
// xcf0: inital(Button pressed) position of Crazyflie
// xm1: next Point after the Crazyflie: the Drone should reach the final height by xm1
......@@ -263,14 +252,24 @@ std::vector<float> xms(3, 0.0);
std::vector<float> direction_ms_to_cf(3, 0.0);
std::vector<float> direction_ms_to_cf_ms_bodyframe(3, 0.0);
float xm2_sign = 1.0;
float xm2_sign[2] = {1.0, 0.0}; // sign for (x,y) in {-1, 0, 1}
//TODO we might want to use float64 instead of float
// trajectory generation variables
double trajectory_start_time = 0;
double total_time_since_start = 0;
bool first_trajectory_calculation = true;
// lookahead for trajectory: fetched from .yaml
float trajectory_deltaT_position = 0;
float trajectory_deltaT_velocity = 0;
float trajectory_duration_1 = 0; // Duration of flight between xcf0 and xm1
float trajectory_duration_2 = 0; // Duration of flight between xm1 and xm2
float trajectory_duration_3 = 0; // Duration of flight between xm2 and xms
float trajectory_t0 = 0; // Time when Button pressed to land on mothership
float trajectory_t1 = 0; // Time at xm1
float trajectory_t2 = 0; // Time at xm2
float trajectory_t3 = 0; // Time at xms
......@@ -297,6 +296,8 @@ float trajectory_velocity_of_CF; // fetched from yaml
float trajectory_total_distance = 0;
float trajectory_duration = 0;
float acceleration[3] = {0.5, 0.5, 0.1};
// returned trajectory setpoint & velocity
......
......@@ -34,7 +34,7 @@ xm2_height_over_ms: 0.3
xm1_height: 0.6
# velocity of trajctory
trajectory_velocity_of_CF : 3.0
trajectory_velocity_of_CF : 2.50
trajectory_velocity_of_CF_init : 1
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
......
......@@ -288,7 +288,6 @@ void buttonPressed_integrator_reset(){
trajectory_duration_2 = 0; // Duration of flight between xm1 and xm2
trajectory_duration_3 = 0; // Duration of flight between xm2 and xms
trajectory_t0 = 0; // Time when Button pressed to land on mothership
trajectory_t1 = 0; // Time at xm1
trajectory_t2 = 0; // Time at xm2
......@@ -371,8 +370,6 @@ void buttonPressed_follow_trajectory(){
//trajectory_y_radius = 0;
first_trajectory_calculation = true;
trajectory_start_time = ros::Time::now().toSec();
trajectory_t0 = 0.0;
//trajectory_t0 = ros::Time::now().toSec();
ROS_INFO_STREAM("trajectory start time: " << trajectory_start_time);
......@@ -556,8 +553,9 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
tookOffFlag = false;
approachedFlag = false;
//bool landedFlag = true;
taking_off_from_ms_height_gain = false;
integratorFlag == DRONEX_INTEGRATOR_OFF;
integratorFlag = DRONEX_INTEGRATOR_OFF;
dronexSetpoint.x = request.ownCrazyflie.x;
dronexSetpoint.y = request.ownCrazyflie.y;
......@@ -576,6 +574,9 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
tookOffFlag = false;
approachedFlag = false;
//bool landedFlag = true;
taking_off_from_ms_height_gain = true;
integratorFlag = DRONEX_INTEGRATOR_OFF;
dronexSetpoint.x = request.ownCrazyflie.x;
dronexSetpoint.y = request.ownCrazyflie.y;
......@@ -586,7 +587,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
case DRONEX_STATE_LAND_ON_MOTHERSHIP:
{
integratorFlag == DRONEX_INTEGRATOR_ON;
integratorFlag = DRONEX_INTEGRATOR_ON;
//if(abs(request.ownCrazyflie.x-request.otherCrazyflies[0].x) < tol_approach[0] &&
// abs(request.ownCrazyflie.y-request.otherCrazyflies[0].y) < tol_approach[1] &&
// abs(request.ownCrazyflie.z-request.otherCrazyflies[0].z + 0.2) < tol_approach[2] ){
......@@ -594,7 +595,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
//ROS_INFO("DRONEX_STATE_LAND_ON_MOTHERSHIP");
dronexSetpoint.x = request.otherCrazyflies[0].x;
dronexSetpoint.y = request.otherCrazyflies[0].y;
dronexSetpoint.z = request.otherCrazyflies[0].z + 0.03;
dronexSetpoint.z = request.otherCrazyflies[0].z + 0.05;
//}else {
//
......@@ -622,10 +623,11 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
{
//ROS_INFO_STREAM("DRONEX_STATE_TAKING_OFF");
thrust_factor = 1;
integratorFlag = DRONEX_INTEGRATOR_ON;
//Check where we start from, if from mothership first go straight up to a certain height
// before aiming the startCoordinates.
if(previous_flying_state == DRONEX_STATE_ON_MOTHERSHIP && taking_off_from_ms_height_gain){
if(/*previous_flying_state == DRONEX_STATE_ON_MOTHERSHIP && */taking_off_from_ms_height_gain){
if(!savedStartCoordinatesFromMS)
{
startCoordinateFromMS[0] = request.otherCrazyflies[0].x;
......@@ -641,44 +643,25 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
}
dronexSetpoint.x = startCoordinateFromMS[0];
dronexSetpoint.y = startCoordinateFromMS[1];
dronexSetpoint.z = startCoordinateFromMS[1] + 0.4;
// For debugging the integrator
// ROS_INFO_STREAM("TO: (x,y,z) Difference: ("
// << request.ownCrazyflie.x-dronexSetpoint.x << ", "
// << request.ownCrazyflie.y-dronexSetpoint.y << ", "
// << request.ownCrazyflie.z-dronexSetpoint.z << ")");
dronexSetpoint.z = startCoordinateFromMS[2] + 0.4;
if(abs(request.ownCrazyflie.x-dronexSetpoint.x) < tol_takeoff[0] && abs(request.ownCrazyflie.y-dronexSetpoint.y) < tol_takeoff[1] &&
abs(request.ownCrazyflie.z-dronexSetpoint.z) < tol_takeoff[2]) {
//tookOffFlag = true;
//ROS_INFO_STREAM("Entering: DRONEX_STATE_HOVER");
//ROS_INFO("took off");
ROS_INFO_STREAM("Entering from MS-takeoff: ground-takeoff");
//previous_flying_state = flying_state;
//flying_state = DRONEX_STATE_HOVER;
//reset_rviz = true;
taking_off_from_ms_height_gain = false;
savedStartCoordinatesFromMS = false;
}
}else{ // previous_flying_state == DRONEX_STATE_ON_MOTHERSHIP
}else{ // previous_flying_state == DRONEX_STATE_ON_MOTHERSHIP
if(!savedStartCoordinates)
{
......@@ -695,18 +678,11 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
}
dronexSetpoint.x = startCoordinateX;
dronexSetpoint.y = startCoordinateY;
dronexSetpoint.z = startCoordinateZ + 0.4;
// For debugging the integrator
// ROS_INFO_STREAM("TO: (x,y,z) Difference: ("
// << request.ownCrazyflie.x-dronexSetpoint.x << ", "
......@@ -718,7 +694,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
ROS_INFO("took off");
tookOffFlag = true;
ROS_INFO_STREAM("Entering: DRONEX_STATE_HOVER");
ROS_INFO_STREAM("Entering from ground-takeoff: DRONEX_STATE_HOVER");
previous_flying_state = flying_state;
flying_state = DRONEX_STATE_HOVER;
taking_off_from_ms_height_gain = false;
......@@ -730,6 +706,8 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
case DRONEX_STATE_HOVER:
{
integratorFlag = DRONEX_INTEGRATOR_ON;
//ROS_INFO_STREAM("DRONEX_STATE_HOVER");
// keep setpoint constant
......@@ -744,6 +722,8 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
break;
case DRONEX_STATE_FOLLOWING_TRAJECTORY:
{
integratorFlag = DRONEX_INTEGRATOR_OFF;
/*if(abs(request.ownCrazyflie.x-request.otherCrazyflies[0].x) < tol_approach[0] &&
abs(request.ownCrazyflie.y-request.otherCrazyflies[0].y) < tol_approach[1] &&
abs(request.ownCrazyflie.z-request.otherCrazyflies[0].z + 0.2) < tol_approach[2] )*/
......@@ -758,7 +738,8 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
flying_state = DRONEX_STATE_LAND_ON_MOTHERSHIP;
ROS_INFO_STREAM("Entering from Trajectory: DRONEX_STATE_LAND_ON_MOTHERSHIP");
} else{
ROS_INFO_STREAM("distance to xm2: "<<request.ownCrazyflie.x-xm2[0]<<", "<<request.ownCrazyflie.y-xm2[1]<<", "<<request.ownCrazyflie.z-xm2[2]);
//ROS_INFO_STREAM("distance to xm2: "<<request.ownCrazyflie.x-xm2[0]<<", "<<request.ownCrazyflie.y-xm2[1]<<", "<<request.ownCrazyflie.z-xm2[2]);
}
}
break;
......@@ -871,7 +852,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
dronexSetpoint.x = request.otherCrazyflies[0].x; // setpoint on mothership
dronexSetpoint.y = request.otherCrazyflies[0].y;
dronexSetpoint.z = request.otherCrazyflies[0].z+0.03;
dronexSetpoint.z = request.otherCrazyflies[0].z+0.05;
dronexSetpoint.yaw = request.otherCrazyflies[0].yaw;
dronexVelocity.x = mothership_vel[0]; // velocity of mothership
......@@ -915,7 +896,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
}
else if(flying_state == DRONEX_STATE_LAND_ON_MOTHERSHIP && (abs(request.ownCrazyflie.x - request.otherCrazyflies[0].x) < tol_land[0]) &&
(abs(request.ownCrazyflie.y - request.otherCrazyflies[0].y) < tol_land[1]) &&
(abs(request.ownCrazyflie.z - 0.03 - request.otherCrazyflies[0].z) < tol_land[2]) ){
(abs(request.ownCrazyflie.z - 0.05 - request.otherCrazyflies[0].z) < tol_land[2]) ){
ROS_INFO("landed -> DRONEX_STATE_ON_MOTHERSHIP");
flying_state = DRONEX_STATE_ON_MOTHERSHIP;
previous_flying_state = DRONEX_STATE_LAND_ON_MOTHERSHIP;
......@@ -1082,7 +1063,7 @@ void calculateTrajectory(Controller::Request &request){
// returns (x,y,z) for a given time t
// returns (x,y,z, vx,vy,vz) for a given time t
// Calculate the trajectory from the Crazyflie to the mothership
// Trajectory: xcf0 -> xm1 -> xm2 -> xms, All segments are straight lines. (For testing purposes) Later should be curves
// xcf0: inital(Button pressed) position of Crazyflie
......@@ -1090,9 +1071,7 @@ void calculateTrajectory(Controller::Request &request){
// xm2: Point behind the mothership
// xms: Point on mothership where we will land
// Please use trajectory_t0 as starting time
// Input: t in [trajectory_t0 and trajectory_t3]
// If t > trajectory_t3 then OUTPUT: (-1,-1,-1) -> exception handling
// Input: t in [0, inf)
std::vector<float> trajectory_to_ms(Controller::Request &request, double t){
// (x,y,z,vx,vy,vz) where abs(vx,vy,vz) = trajectory_velocity_of_CF
......@@ -1141,34 +1120,56 @@ std::vector<float> trajectory_to_ms(Controller::Request &request, double t){
//TODO Attention this conversion might be buggy
direction_ms_to_cf_ms_bodyframe[0] = cosYaw*direction_ms_to_cf[0];
direction_ms_to_cf_ms_bodyframe[1] = sinYaw*direction_ms_to_cf[1];
direction_ms_to_cf_ms_bodyframe[0] = cosYaw*direction_ms_to_cf[0] + sinYaw*direction_ms_to_cf[1];
direction_ms_to_cf_ms_bodyframe[1] = cosYaw*direction_ms_to_cf[1] - sinYaw*direction_ms_to_cf[0];
if(direction_ms_to_cf_ms_bodyframe[0] < 0.0){
xm2_sign = 1.0; // Crazyflie can land from behind
}else{
xm2_sign = -1.0; // Crazyflie can land from front
// if(direction_ms_to_cf_ms_bodyframe[0] < 0.0){
// xm2_sign = -1.0; // Crazyflie can land from behind
// }else{
// xm2_sign = 1.0; // Crazyflie can land from front
// }
if(direction_ms_to_cf_ms_bodyframe[0] > abs(direction_ms_to_cf_ms_bodyframe[1])){
xm2_sign[0] = 1.0;
xm2_sign[1] = 0.0; // Crazyflie can land from behind
}
else if(direction_ms_to_cf_ms_bodyframe[1] > abs(direction_ms_to_cf_ms_bodyframe[0])){
xm2_sign[0] = 0.0;
xm2_sign[1] = -1.0; // Crazyflie can land from front
}
else if(-direction_ms_to_cf_ms_bodyframe[0] > abs(direction_ms_to_cf_ms_bodyframe[1])){
xm2_sign[0] = -1.0;
xm2_sign[1] = 0.0; // Crazyflie can land from front
}
else if(-direction_ms_to_cf_ms_bodyframe[1] > abs(direction_ms_to_cf_ms_bodyframe[0])){
xm2_sign[0] = 0.0;
xm2_sign[1] = 1.0; // Crazyflie can land from front
}
// Calculate xm2(the point behind the mothership)
// absolute velocity of MS
float ms_abs_velocity = sqrt(mothership_vel[0] * mothership_vel[0] + mothership_vel[1] * mothership_vel[1] + mothership_vel[2] * mothership_vel[2]);
float normalized_ms_velocity = ms_abs_velocity/xm2_distance_to_ms_at_zero_velocity;
// scale absolute velocity
float normalized_ms_velocity = ms_abs_velocity / xm2_distance_to_ms_at_zero_velocity;
// clip if too big
if(normalized_ms_velocity > xm2_distance_to_ms_at_zero_velocity){
normalized_ms_velocity = xm2_distance_to_ms_at_zero_velocity;
}
xm2_distance_to_ms = xm2_distance_to_ms_at_zero_velocity - xm2_distance_to_ms_scaling_factor * normalized_ms_velocity;
xm2[0] = request.otherCrazyflies[0].x - xm2_sign*cosYaw*xm2_distance_to_ms;
xm2[1] = request.otherCrazyflies[0].y - xm2_sign*sinYaw*xm2_distance_to_ms;
xm2[0] = request.otherCrazyflies[0].x + (xm2_sign[0] * cosYaw + xm2_sign[1] * sinYaw) * xm2_distance_to_ms;
xm2[1] = request.otherCrazyflies[0].y + (-xm2_sign[1] * cosYaw + xm2_sign[0] * sinYaw) * xm2_distance_to_ms;
xm2[2] = request.otherCrazyflies[0].z + xm2_height_over_ms;
xms[0] = request.otherCrazyflies[0].x;
xms[1] = request.otherCrazyflies[0].y;
xms[2] = request.otherCrazyflies[0].z+0.2; // TODO add a certain height to not collide with mothership
xms[2] = request.otherCrazyflies[0].z+0.2; // a bit over the MS not to collide
// Calculate the whole trajectory distance
......@@ -1188,7 +1189,7 @@ std::vector<float> trajectory_to_ms(Controller::Request &request, double t){
trajectory_duration_3 = calculate_distance_in_xyz(xm2,xms)/trajectory_velocity_of_CF;
// Time when at xm1
trajectory_t1 = trajectory_t0 + trajectory_duration_1;
trajectory_t1 = trajectory_duration_1;
// Time when at xm2
trajectory_t2 = trajectory_t1 + trajectory_duration_2;
// Time when at xms
......@@ -1198,7 +1199,7 @@ std::vector<float> trajectory_to_ms(Controller::Request &request, double t){
// Calculate the x,y,z position depending on where the CF should be at a certain time t
if(trajectory_t0 <= t && t <= trajectory_t1){ // Calculate position & velocity on line between xcf0 and xm1
if(0.0 <= t && t <= trajectory_t1){ // Calculate position & velocity on line between xcf0 and xm1
for(int i = 0; i < 3; i++){
trajectory_return[i] = xcf0[i] + t*(xm1[i] - xcf0[i])/trajectory_t1;
trajectory_return[i+3] = (xm1[i]-xcf0[i])/calculate_distance_in_xyz(xcf0, xm1) * trajectory_velocity_of_CF_init;
......@@ -1212,8 +1213,14 @@ std::vector<float> trajectory_to_ms(Controller::Request &request, double t){
for(int i = 0; i < 3; i++){
trajectory_return[i] = xm2[i] + (t - trajectory_t2)*(xms[i] - xm2[i])/(trajectory_t3 - trajectory_t2);
trajectory_return[i+3] = (xms[i]-xm2[i])/calculate_distance_in_xyz(xms, xm2) * trajectory_velocity_of_CF;
//float acceleration = (mothership_vel[i] - trajectory_return[i+3]) / (trajectory_t3 - trajectory_t2);
// smooth velocity change between xm2 and xms
//trajectory_return[i+3] = trajectory_return[i+3] + (t - trajectory_t2) * (mothership_vel[i] - trajectory_return[i+3]) / (trajectory_t3 - trajectory_t2);
}
}else{
trajectory_return[0] = xms[0];
trajectory_return[1] = xms[1];
trajectory_return[2] = xms[2];
......@@ -1285,7 +1292,6 @@ void motorsOFFSmooth(Controller::Response &response){
}
void motorsOFF(Controller::Response &response){
float feed_forward_thrust_per_motor = m_mass_total_grams * 9.81/(1000*4);
// > Put in the per motor commands
response.controlOutput.motorCmd1 = 0;
response.controlOutput.motorCmd2 = 0;
......@@ -1821,6 +1827,7 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle
rviz_points_trajectory_publisher.publish(rviz_mothership);
//if (ros::ok()){
//ROS_INFO("Init RVIZ STUFF");
geometry_msgs::Point p;
......@@ -1870,9 +1877,9 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle
xm2_position_list.type = visualization_msgs::Marker::SPHERE_LIST;
xm2_position_list.scale.x = 0.01;
xm2_position_list.scale.y = 0.01;
xm2_position_list.scale.z = 0.01;
xm2_position_list.scale.x = 0.05;
xm2_position_list.scale.y = 0.05;
xm2_position_list.scale.z = 0.05;
xm2_position_list.color.r = 0.0f;
xm2_position_list.color.g = 0.0f;
......
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