Commit 7d589ba8 authored by mastefan's avatar mastefan
Browse files

trajectory tracking and rviz

parent d1e38f43
...@@ -48,7 +48,9 @@ ...@@ -48,7 +48,9 @@
#include <math.h> #include <math.h>
#include <stdlib.h> #include <stdlib.h>
#include "ros/ros.h" #include "ros/ros.h"
#include <ros/package.h> #include "ros/package.h"
#include "visualization_msgs/Marker.h"
//the generated structs from the msg-files have to be included //the generated structs from the msg-files have to be included
#include "d_fall_pps/ViconData.h" #include "d_fall_pps/ViconData.h"
...@@ -69,6 +71,7 @@ ...@@ -69,6 +71,7 @@
// ---------------------------------------------------------------------------------- // ----------------------------------------------------------------------------------
// DDDD EEEEE FFFFF III N N EEEEE SSSS // DDDD EEEEE FFFFF III N N EEEEE SSSS
// D D E F I NN N E S // D D E F I NN N E S
...@@ -258,10 +261,10 @@ float trajectory_t2 = 0; // Time at xm2 ...@@ -258,10 +261,10 @@ float trajectory_t2 = 0; // Time at xm2
float trajectory_t3 = 0; // Time at xms float trajectory_t3 = 0; // Time at xms
float xm1_x_distance = 0.5; float xm1_x_distance = 0.5;
float xm2_distance_to_ms = 0.3; float xm2_distance_to_ms = 0.6;
// Calculate from approximate distance and velocity the total duration of the trajectory // Calculate from approximate distance and velocity the total duration of the trajectory
float trajectory_velocity_of_CF = 1.0; // Fly at 1 m/s float trajectory_velocity_of_CF = 2.0; // Fly at 1 m/s
float trajectory_total_distance = 0; float trajectory_total_distance = 0;
float trajectory_duration = 0; float trajectory_duration = 0;
...@@ -316,7 +319,7 @@ float m_xAdjustment = 0.0f; ...@@ -316,7 +319,7 @@ float m_xAdjustment = 0.0f;
float m_yAdjustment = 0.0f; float m_yAdjustment = 0.0f;
// Boolean for whether to limit rate of change of the setpoint // Boolean for whether to limit rate of change of the setpoint
bool m_shouldSmoothSetpointChanges = true; bool m_shouldSmoothSetpointChanges = false;
bool m_shouldClipVelocity = true; bool m_shouldClipVelocity = true;
// Max setpoint change per second // Max setpoint change per second
...@@ -458,6 +461,10 @@ ros::Publisher dronexSetpointToGUIPublisher; ...@@ -458,6 +461,10 @@ ros::Publisher dronexSetpointToGUIPublisher;
// publisher for flying state // publisher for flying state
ros::Publisher flyingStatePublisher; ros::Publisher flyingStatePublisher;
visualization_msgs::Marker drone_position_list;
ros::Publisher rviz_points_publisher;
// RELEVANT NOTES ABOUT THE VARIABLES DECLARE HERE: // RELEVANT NOTES ABOUT THE VARIABLES DECLARE HERE:
// The "CrazyflieData" type used for the "request" variable is a // The "CrazyflieData" type used for the "request" variable is a
// structure as defined in the file "CrazyflieData.msg" which has the following // structure as defined in the file "CrazyflieData.msg" which has the following
......
...@@ -543,7 +543,8 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response & ...@@ -543,7 +543,8 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
abs(request.ownCrazyflie.y-request.otherCrazyflies[0].y) < tol_approach[1] && abs(request.ownCrazyflie.y-request.otherCrazyflies[0].y) < tol_approach[1] &&
abs(request.ownCrazyflie.z-request.otherCrazyflies[0].z + 0.2) < tol_approach[2] ){ abs(request.ownCrazyflie.z-request.otherCrazyflies[0].z + 0.2) < tol_approach[2] ){
flying_state = DRONEX_STATE_LAND_ON_MOTHERSHIP; //flying_state = DRONEX_STATE_LAND_ON_MOTHERSHIP;
ROS_INFO_STREAM("Entering from Trajectory: DRONEX_STATE_LAND_ON_MOTHERSHIP");
} }
break; break;
...@@ -631,6 +632,8 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response & ...@@ -631,6 +632,8 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
dronexVelocity.y = trajectory_velocity[1]; dronexVelocity.y = trajectory_velocity[1];
dronexVelocity.z = trajectory_velocity[2]; dronexVelocity.z = trajectory_velocity[2];
/* /*
m_setpoint_for_controller_2[0] = request.owncraazyflie.x; m_setpoint_for_controller_2[0] = request.owncraazyflie.x;
m_setpoint_for_controller_2[1] = request.owncraazyflie.y; m_setpoint_for_controller_2[1] = request.owncraazyflie.y;
...@@ -648,9 +651,9 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response & ...@@ -648,9 +651,9 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
//dronexSetpoint.z = dronexSetpoint.z; //dronexSetpoint.z = dronexSetpoint.z;
dronexSetpoint.yaw = request.otherCrazyflies[0].yaw; // same yaw as mothership dronexSetpoint.yaw = request.otherCrazyflies[0].yaw; // same yaw as mothership
dronexVelocity.x = mothership_vel[0]; dronexVelocity.x = mothership_vel[0]*0;
dronexVelocity.y = mothership_vel[1]; dronexVelocity.y = mothership_vel[1]*0;
dronexVelocity.z = mothership_vel[2]; dronexVelocity.z = mothership_vel[2]*0;
} }
} }
...@@ -780,7 +783,8 @@ void calculateTrajectory(Controller::Request &request){ ...@@ -780,7 +783,8 @@ void calculateTrajectory(Controller::Request &request){
total_time_since_start = ros::Time::now().toSec() - trajectory_start_time; total_time_since_start = ros::Time::now().toSec() - trajectory_start_time;
float trajectory_temp_setpoint[3] = trajectory_to_ms(request, total_time_since_start); std::vector<float> trajectory_temp_setpoint(3);
trajectory_temp_setpoint = trajectory_to_ms(request, total_time_since_start);
trajectory_setpoint[0] = trajectory_temp_setpoint[0]; trajectory_setpoint[0] = trajectory_temp_setpoint[0];
trajectory_setpoint[1] = trajectory_temp_setpoint[1]; trajectory_setpoint[1] = trajectory_temp_setpoint[1];
trajectory_setpoint[2] = trajectory_temp_setpoint[2]; trajectory_setpoint[2] = trajectory_temp_setpoint[2];
...@@ -790,7 +794,8 @@ void calculateTrajectory(Controller::Request &request){ ...@@ -790,7 +794,8 @@ void calculateTrajectory(Controller::Request &request){
float deltaT = 0.2; float deltaT = 0.2;
float trajectory_temp_setpoint_future[3] = trajectory_to_ms(request, total_time_since_start + deltaT); std::vector<float> trajectory_temp_setpoint_future(3);
trajectory_temp_setpoint_future = trajectory_to_ms(request, total_time_since_start + deltaT);
trajectory_velocity[0] = (trajectory_temp_setpoint_future[0]-request.ownCrazyflie.x)/deltaT; trajectory_velocity[0] = (trajectory_temp_setpoint_future[0]-request.ownCrazyflie.x)/deltaT;
trajectory_velocity[1] = (trajectory_temp_setpoint_future[1]-request.ownCrazyflie.y)/deltaT; trajectory_velocity[1] = (trajectory_temp_setpoint_future[1]-request.ownCrazyflie.y)/deltaT;
trajectory_velocity[2] = 0; trajectory_velocity[2] = 0;
...@@ -827,6 +832,9 @@ void calculateTrajectory(Controller::Request &request){ ...@@ -827,6 +832,9 @@ void calculateTrajectory(Controller::Request &request){
trajectory_velocity[0] = trajectory_velocity[0]*tot_real_velocity_xy/tot_velocity_xy; trajectory_velocity[0] = trajectory_velocity[0]*tot_real_velocity_xy/tot_velocity_xy;
trajectory_velocity[1] = trajectory_velocity[1]*tot_real_velocity_xy/tot_velocity_xy; trajectory_velocity[1] = trajectory_velocity[1]*tot_real_velocity_xy/tot_velocity_xy;
*/ */
trajectory_velocity[0] = 0;
trajectory_velocity[1] = 0;
} }
...@@ -1435,14 +1443,80 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle ...@@ -1435,14 +1443,80 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle
debugMsg.value_2 = mothership_vel[1]; debugMsg.value_2 = mothership_vel[1];
debugMsg.value_3 = mothership_vel[2]; debugMsg.value_3 = mothership_vel[2];
debugMsg.value_4 = thrust_factor; debugMsg.value_4 = thrust_factor;
debugMsg.value_5 = trajectory_setpoint[0]; debugMsg.value_5 = dronexSetpoint.x;
debugMsg.value_6 = trajectory_setpoint[1]; debugMsg.value_6 = dronexSetpoint.y;
debugMsg.value_7 = trajectory_setpoint[2]; debugMsg.value_7 = dronexSetpoint.z;
// ...................... // ......................
// debugMsg.value_10 = your_variable_name; // debugMsg.value_10 = your_variable_name;
// Publish the "debugMsg" // Publish the "debugMsg"
debugPublisher.publish(debugMsg); debugPublisher.publish(debugMsg);
if (ros::ok()){
ROS_INFO("Init RVIZ STUFF");
geometry_msgs::Point p;
p.x = request.ownCrazyflie.x;
p.y = request.ownCrazyflie.y;
p.z = request.ownCrazyflie.z;
drone_position_list.header.frame_id = "/frame_Trajectory";
drone_position_list.header.stamp = ros::Time::now();
drone_position_list.ns = "spheres";
drone_position_list.action = visualization_msgs::Marker::ADD;
drone_position_list.pose.orientation.w = 1.0;
drone_position_list.id = 0;
drone_position_list.type = visualization_msgs::Marker::SPHERE_LIST;
drone_position_list.scale.x = 0.01;
drone_position_list.scale.y = 0.01;
drone_position_list.scale.z = 0.01;
drone_position_list.color.r = 1.0f;
drone_position_list.color.a = 1.0;
drone_position_list.points.push_back(p);
rviz_points_publisher.publish(drone_position_list);
p.x = dronexSetpoint.x;
p.y = dronexSetpoint.y;
p.z = dronexSetpoint.z;
drone_position_list.header.frame_id = "/frame_Trajectory";
drone_position_list.header.stamp = ros::Time::now();
drone_position_list.ns = "spheres";
drone_position_list.action = visualization_msgs::Marker::ADD;
drone_position_list.pose.orientation.w = 1.0;
drone_position_list.id = 0;
drone_position_list.type = visualization_msgs::Marker::SPHERE_LIST;
drone_position_list.scale.x = 0.01;
drone_position_list.scale.y = 0.01;
drone_position_list.scale.z = 0.01;
drone_position_list.color.r = 0.0f;
drone_position_list.color.g = 1.0f;
drone_position_list.color.a = 1.0;
drone_position_list.points.push_back(p);
rviz_points_publisher.publish(drone_position_list);
}
} }
...@@ -2207,6 +2281,32 @@ int main(int argc, char* argv[]) { ...@@ -2207,6 +2281,32 @@ int main(int argc, char* argv[]) {
// Print out some information to the user. // Print out some information to the user.
ROS_INFO("[DroneX CONTROLLER] Service ready :-)"); ROS_INFO("[DroneX CONTROLLER] Service ready :-)");
rviz_points_publisher = nodeHandle.advertise<visualization_msgs::Marker>("rviz_trajectory", 1);
/*if (ros::ok()){
drone_position_list.header.frame_id = "/frame_Trajectory";
drone_position_list.header.stamp = ros::Time::now();
drone_position_list.ns = "spheres";
drone_position_list.action = visualization_msgs::Marker::ADD;
drone_position_list.pose.orientation.w = 1.0;
drone_position_list.id = 0;
drone_position_list.type = visualization_msgs::Marker::SPHERE_LIST;
drone_position_list.scale.x = 0.1;
drone_position_list.scale.y = 0.1;
drone_position_list.scale.z = 0.1;
drone_position_list.color.r = 1.0f;
drone_position_list.color.a = 1.0;
}*/
// Enter an endless while loop to keep the node alive. // Enter an endless while loop to keep the node alive.
ros::spin(); ros::spin();
......
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