To receive notifications about scheduled maintenance, please subscribe to the mailing-list gitlab-operations@sympa.ethz.ch. You can subscribe to the mailing-list at https://sympa.ethz.ch

Commit 3b7eb375 authored by mastefan's avatar mastefan
Browse files

XM2 on rviz, rviz reset

parent d37fef4e
......@@ -303,6 +303,9 @@ d_fall_pps::AreaBounds area;
// thrust factor to do a smooth landing
float thrust_factor = 1;
bool motorsOFFBool = false;
double motorsOFFStartTime;
......@@ -479,12 +482,14 @@ ros::Publisher dronexSetpointToGUIPublisher;
ros::Publisher flyingStatePublisher;
visualization_msgs::Marker drone_position_list;
visualization_msgs::Marker xm2_position_list;
visualization_msgs::Marker setpoint_position_list;
visualization_msgs::Marker rviz_mothership;
ros::Publisher rviz_points_trajectory_publisher;
ros::Publisher rviz_points_trajectory_generated_publisher;
ros::Publisher rviz_xm2_publisher;
bool reset_rviz = false;
......@@ -576,6 +581,7 @@ void setarrayzero4(float arr[4], int n);
void crazyfliecontextRefresh(d_fall_pps::CrazyflieContext context);
void motorsOFF(Controller::Response &response);
void motorsOFFSmooth(Controller::Response &response);
......
......@@ -44,7 +44,7 @@ xm2_distance_to_ms_scaling_factor : 1 # Scale in direction of the mothersh
# tolerances
tol_takeoff: [0.07, 0.07, 0.07]
tol_approach: [0.2, 0.2, 0.1]
tol_approach: [0.3, 0.3, 0.1]
tol_land: [0.07, 0.07, 0.04]
......
......@@ -615,6 +615,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
case DRONEX_STATE_TAKING_OFF:
{
//ROS_INFO_STREAM("DRONEX_STATE_TAKING_OFF");
thrust_factor = 1;
if(!savedStartCoordinates)
{
......@@ -841,7 +842,10 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
flying_state = DRONEX_STATE_ON_MOTHERSHIP;
}
if(flying_state == DRONEX_STATE_GROUND || flying_state == DRONEX_STATE_ON_MOTHERSHIP){
if(flying_state == DRONEX_STATE_ON_MOTHERSHIP){
motorsOFF(response);
ROS_INFO("On Mothership");
}else if(flying_state == DRONEX_STATE_GROUND){
motorsOFF(response);
}
else{
......@@ -1157,20 +1161,49 @@ float calculate_distance_in_xyz(std::vector<float> p1, std::vector<float> p2){
// Set motors Output to 0
void motorsOFFSmooth(Controller::Response &response){
float feed_forward_thrust_per_motor = m_mass_total_grams * 9.81/(1000*4);
if(!motorsOFFBool){
motorsOFFStartTime = ros::Time::now().toSec();
motorsOFFBool = true;
}
if(ros::Time::now().toSec() - motorsOFFStartTime > 5.0){
response.controlOutput.motorCmd1 = 0;
response.controlOutput.motorCmd2 = 0;
response.controlOutput.motorCmd3 = 0;
response.controlOutput.motorCmd4 = 0;
}else{
//> Put in the per motor commands
response.controlOutput.motorCmd1 = 0.55* computeMotorPolyBackward(feed_forward_thrust_per_motor);
response.controlOutput.motorCmd2 = 0.55* computeMotorPolyBackward(feed_forward_thrust_per_motor);
response.controlOutput.motorCmd3 = 0.55* computeMotorPolyBackward(feed_forward_thrust_per_motor);
response.controlOutput.motorCmd4 = 0.55* computeMotorPolyBackward(feed_forward_thrust_per_motor);
}
// Specify that this controller is a rate controller
// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
}
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.8 */0* computeMotorPolyBackward(feed_forward_thrust_per_motor);
response.controlOutput.motorCmd2 = /*0.8 */0* computeMotorPolyBackward(feed_forward_thrust_per_motor);
response.controlOutput.motorCmd3 = /*0.8 */0* computeMotorPolyBackward(feed_forward_thrust_per_motor);
response.controlOutput.motorCmd4 = /*0.8 */0* computeMotorPolyBackward(feed_forward_thrust_per_motor);
response.controlOutput.motorCmd1 = 0;
response.controlOutput.motorCmd2 = 0;
response.controlOutput.motorCmd3 = 0;
response.controlOutput.motorCmd4 = 0;
// Specify that this controller is a rate controller
// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
}
}
// Estimate mothership velocity
......@@ -1649,6 +1682,11 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle
// Publish the "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);
......@@ -1712,11 +1750,47 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle
drone_position_list.color.b = blue_1;
drone_position_list.color.a = 1.0f;
drone_position_list.points.push_back(p);
rviz_points_trajectory_publisher.publish(drone_position_list);
geometry_msgs::Point q;
q.x = xm2[0];
q.y = xm2[1];
q.z = xm2[2];
xm2_position_list.header.frame_id = "/frame_Trajectory";
xm2_position_list.header.stamp = ros::Time::now();
xm2_position_list.ns = "spheres";
xm2_position_list.action = visualization_msgs::Marker::ADD;
xm2_position_list.pose.orientation.w = 1.0;
xm2_position_list.id = 0;
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.color.r = 0.0f;
xm2_position_list.color.g = 0.0f;
xm2_position_list.color.b = 1.0f;
xm2_position_list.color.a = 1.0f;
xm2_position_list.points.push_back(q);
rviz_xm2_publisher.publish(xm2_position_list);
p.x = dronexSetpoint.x;
p.y = dronexSetpoint.y;
p.z = dronexSetpoint.z;
......@@ -1751,9 +1825,21 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle
while(setpoint_position_list.points.size() > 0){
setpoint_position_list.points.erase(setpoint_position_list.points.begin());
}
while(xm2_position_list.points.size() > 0){
xm2_position_list.points.erase(xm2_position_list.points.begin());
}
reset_rviz = false;
}
/*if(drone_position_list.points.size() > 1000){
drone_position_list.points.erase(drone_position_list.points.begin());
setpoint_position_list.points.erase(setpoint_position_list.points.begin());
xm2_position_list.points.erase(xm2_position_list.points.begin());
}*/
rviz_points_trajectory_generated_publisher.publish(setpoint_position_list);
......@@ -2554,6 +2640,7 @@ int main(int argc, char* argv[]) {
rviz_points_trajectory_publisher = nodeHandle.advertise<visualization_msgs::Marker>("rviz_trajectory", 1);
rviz_points_trajectory_generated_publisher = nodeHandle.advertise<visualization_msgs::Marker>("rviz_trajectory_generated", 1);
rviz_xm2_publisher = nodeHandle.advertise<visualization_msgs::Marker>("rviz_xm2", 1);
/*if (ros::ok()){
......
Markdown is supported
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