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

trajectory tracking and rviz

parent d1e38f43
......@@ -48,7 +48,9 @@
#include <math.h>
#include <stdlib.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
#include "d_fall_pps/ViconData.h"
......@@ -69,6 +71,7 @@
// ----------------------------------------------------------------------------------
// DDDD EEEEE FFFFF III N N EEEEE SSSS
// D D E F I NN N E S
......@@ -258,10 +261,10 @@ float trajectory_t2 = 0; // Time at xm2
float trajectory_t3 = 0; // Time at xms
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
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_duration = 0;
......@@ -316,7 +319,7 @@ float m_xAdjustment = 0.0f;
float m_yAdjustment = 0.0f;
// Boolean for whether to limit rate of change of the setpoint
bool m_shouldSmoothSetpointChanges = true;
bool m_shouldSmoothSetpointChanges = false;
bool m_shouldClipVelocity = true;
// Max setpoint change per second
......@@ -458,6 +461,10 @@ ros::Publisher dronexSetpointToGUIPublisher;
// publisher for flying state
ros::Publisher flyingStatePublisher;
visualization_msgs::Marker drone_position_list;
ros::Publisher rviz_points_publisher;
// RELEVANT NOTES ABOUT THE VARIABLES DECLARE HERE:
// The "CrazyflieData" type used for the "request" variable is a
// structure as defined in the file "CrazyflieData.msg" which has the following
......
......@@ -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.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;
......@@ -631,6 +632,8 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
dronexVelocity.y = trajectory_velocity[1];
dronexVelocity.z = trajectory_velocity[2];
/*
m_setpoint_for_controller_2[0] = request.owncraazyflie.x;
m_setpoint_for_controller_2[1] = request.owncraazyflie.y;
......@@ -648,9 +651,9 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
//dronexSetpoint.z = dronexSetpoint.z;
dronexSetpoint.yaw = request.otherCrazyflies[0].yaw; // same yaw as mothership
dronexVelocity.x = mothership_vel[0];
dronexVelocity.y = mothership_vel[1];
dronexVelocity.z = mothership_vel[2];
dronexVelocity.x = mothership_vel[0]*0;
dronexVelocity.y = mothership_vel[1]*0;
dronexVelocity.z = mothership_vel[2]*0;
}
}
......@@ -780,7 +783,8 @@ void calculateTrajectory(Controller::Request &request){
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[1] = trajectory_temp_setpoint[1];
trajectory_setpoint[2] = trajectory_temp_setpoint[2];
......@@ -790,7 +794,8 @@ void calculateTrajectory(Controller::Request &request){
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[1] = (trajectory_temp_setpoint_future[1]-request.ownCrazyflie.y)/deltaT;
trajectory_velocity[2] = 0;
......@@ -827,6 +832,9 @@ void calculateTrajectory(Controller::Request &request){
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[0] = 0;
trajectory_velocity[1] = 0;
}
......@@ -1435,14 +1443,80 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle
debugMsg.value_2 = mothership_vel[1];
debugMsg.value_3 = mothership_vel[2];
debugMsg.value_4 = thrust_factor;
debugMsg.value_5 = trajectory_setpoint[0];
debugMsg.value_6 = trajectory_setpoint[1];
debugMsg.value_7 = trajectory_setpoint[2];
debugMsg.value_5 = dronexSetpoint.x;
debugMsg.value_6 = dronexSetpoint.y;
debugMsg.value_7 = dronexSetpoint.z;
// ......................
// debugMsg.value_10 = your_variable_name;
// Publish the "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[]) {
// Print out some information to the user.
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.
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