diff --git a/quad_control/CMakeLists.txt b/quad_control/CMakeLists.txt
index 22d0b42..4d889f4 100644
--- a/quad_control/CMakeLists.txt
+++ b/quad_control/CMakeLists.txt
@@ -15,10 +15,11 @@ find_package(catkin REQUIRED COMPONENTS
sensor_msgs
planning_msgs
tf
+ costmap_2d
)
## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
+find_package(Boost REQUIRED)
## Uncomment this if the package has a setup.py. This macro ensures
@@ -109,8 +110,8 @@ find_package(catkin REQUIRED COMPONENTS
catkin_package(
INCLUDE_DIRS include
LIBRARIES quad_controller
- CATKIN_DEPENDS cmake_modules geometry_msgs glog_catkin mav_msgs roscpp sensor_msgs planning_msgs tf
-# DEPENDS system_lib
+ CATKIN_DEPENDS cmake_modules geometry_msgs glog_catkin mav_msgs roscpp sensor_msgs planning_msgs costmap_2d tf
+ DEPENDS Boost
)
###########
@@ -120,7 +121,7 @@ catkin_package(
## Specify additional locations of header files
## Your package locations should be listed before other locations
# include_directories(include)
-include_directories(include ${catkin_INCLUDE_DIRS}
+include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}
)
## Declare a C++ library
@@ -153,7 +154,7 @@ add_dependencies(quad_controller ${catkin_EXPORTED_TARGETS})
add_executable(waypoint_publisher_node src/nodes/waypoint_publisher_node.cpp)
add_dependencies(waypoint_publisher_node ${catkin_EXPORTED_TARGETS})
-target_link_libraries(waypoint_publisher_node quad_controller ${catkin_LIBRARIES})
+target_link_libraries(waypoint_publisher_node quad_controller ${catkin_LIBRARIES} ${Boost_LIBRARIES})
add_executable(position_controller_node src/nodes/position_controller_node.cpp)
add_dependencies(position_controller_node ${catkin_EXPORTED_TARGETS})
diff --git a/quad_control/package.xml b/quad_control/package.xml
index e6a5cd0..ff74c65 100644
--- a/quad_control/package.xml
+++ b/quad_control/package.xml
@@ -48,6 +48,7 @@
sensor_msgs
planning_msgs
tf
+ costmap_2d
cmake_modules
geometry_msgs
glog_catkin
@@ -56,6 +57,7 @@
sensor_msgs
planning_msgs
tf
+ costmap_2d
diff --git a/quad_control/src/nodes/waypoint_publisher_node.cpp b/quad_control/src/nodes/waypoint_publisher_node.cpp
index 799a545..0e8a59d 100644
--- a/quad_control/src/nodes/waypoint_publisher_node.cpp
+++ b/quad_control/src/nodes/waypoint_publisher_node.cpp
@@ -19,12 +19,16 @@
*/
#include "waypoint_publisher_node.h"
+#include
+#include
+#include
+#include
namespace quad_control {
std::vector WaypointWithTime::Read_waypoints(std::vector waypoints){
- std::ifstream wp_file("/home/wil/ros/catkin_ws/src/arducopter_slam/quad_control/resource/kitchen_short_waypoints.txt");
+ std::ifstream wp_file("/home/developer/Development/ros/src/ROS_quadrotor_simulator/quad_control/resource/kitchen_short_waypoints.txt");
//wg_waypoints.txt kitchen_waypoints.txt
if (wp_file.is_open()) {
@@ -45,6 +49,70 @@ std::vector WaypointWithTime::Read_waypoints(std
}
+quad_control::WaypointWithTime WaypointWithTime::PointToWaypointWithTime(const geometry_msgs::Point32& point) {
+ const static float yaw = 90.0;
+ const static double time = 2.5;
+ return WaypointWithTime(time, point.x, point.y, point.z, yaw * DEG_2_RAD);
+}
+
+/**
+* @brief Calculate distance between two points
+* @param one Point one
+* @param two Point two
+* @return Distance between two points
+*/
+template
+double pointsDistance(const T &one, const S &two){
+ return sqrt(pow(one.x-two.x,2.0) + pow(one.y-two.y,2.0) + pow(one.z-two.z,2.0));
+}
+
+/**
+* @brief Calculate polygon perimeter
+* @param polygon Polygon to process
+* @return Perimeter of polygon
+*/
+double polygonPerimeter(const geometry_msgs::Polygon &polygon){
+ double perimeter = 0;
+ if(polygon.points.size() > 1)
+ {
+ for (int i = 0, j = polygon.points.size() - 1; i < polygon.points.size(); j = i++)
+ {
+ perimeter += pointsDistance(polygon.points[i], polygon.points[j]);
+ }
+ }
+ return perimeter;
+}
+
+/**
+* @brief Evaluate whether two points are approximately adjacent, within a specified proximity distance.
+* @param one Point one
+* @param two Point two
+* @param proximity Proximity distance
+* @return True if approximately adjacent, false otherwise
+*/
+template
+bool pointsNearby(const T &one, const S &two, const double &proximity){
+ return pointsDistance(one, two) <= proximity;
+}
+
+/**
+* @brief Evaluate if point is inside area defined by polygon. Undefined behaviour for points on line.
+* @param point Point to test
+* @param polygon Polygon to test
+* @return True if point is inside polygon, false otherwise
+*/
+template
+bool pointInPolygon(const T &point, const geometry_msgs::Polygon &polygon){
+ int cross = 0;
+ for (int i = 0, j = polygon.points.size()-1; i < polygon.points.size(); j = i++) {
+ if ( ((polygon.points[i].y > point.y) != (polygon.points[j].y>point.y)) &&
+ (point.x < (polygon.points[j].x-polygon.points[i].x) * (point.y-polygon.points[i].y) / (polygon.points[j].y-polygon.points[i].y) + polygon.points[i].x) ){
+ cross++;
+ }
+ }
+ return bool(cross % 2);
+}
+
WaypointPublisherNode::WaypointPublisherNode(){
InitializeParams();
@@ -56,9 +124,12 @@ WaypointPublisherNode::WaypointPublisherNode(){
odometry_sub_ = nh.subscribe("ground_truth/odometry", 10, &WaypointPublisherNode::OdometryCallback, this);
cmd_vel_sub_ = nh.subscribe("/cmd_vel", 10, &WaypointPublisherNode::CommandVelCallback, this);
cmd_threednav_sub_ = nh.subscribe("/cmd_3dnav", 10, &WaypointPublisherNode::threedNavCallback, this);
+ live_waypoint_sub_ = nh.subscribe("/clicked_point", 100, &WaypointPublisherNode::wayPointCallback, this);
//Publisher
trajectory_pub = nh.advertise("command/waypoint", 10);
+ point_viz_pub_ = nh.advertise("waypoint_publisher_node_polygon_marker", 10);
+ point_viz_timer_ = nh.createWallTimer(ros::WallDuration(0.1), boost::bind(&WaypointPublisherNode::vizPubCallback, this));
ROS_INFO_ONCE("Started Waypoint Publisher.");
@@ -86,6 +157,11 @@ void WaypointPublisherNode::InitializeParams(){
threedNav_trajectory.position(1) = 0.0;
threedNav_trajectory.position(2) = 0.0;
+ //Visualization
+ input_.header.frame_id = "map";
+ waiting_for_center_ = false;
+ waypoints_ready = false;
+
ROS_INFO_ONCE("Waypoint_publisher_node Paramters Initialized.");
}
@@ -116,6 +192,172 @@ void WaypointPublisherNode::threedNavCallback(const mav_msgs::CommandTrajectoryC
}
+void WaypointPublisherNode::wayPointCallback(const geometry_msgs::PointStampedConstPtr& pointStampped)
+{
+ printf("I received point: (%f, %f, %f)\n", pointStampped->point.x, pointStampped->point.y, pointStampped->point.z);
+
+ double average_distance = polygonPerimeter(input_.polygon) / input_.polygon.points.size();
+
+ if(waiting_for_center_){
+ //flag is set so this is the last point of boundary polygon, i.e. center
+ if(!pointInPolygon(pointStampped->point,input_.polygon)){
+ waypoints_ready = false;
+ ROS_ERROR("Center not inside polygon, restarting");
+ }else{
+ // Notify way points for drones
+ std::cout << "Path complete, will update waypoints for drones!\n";
+ waypoints.clear();
+ for(const auto& point : input_.polygon.points) {
+ waypoints.push_back(waypoint_utility.PointToWaypointWithTime(point));
+ }
+ int size = waypoints.size();
+ printf("Start publishing #%d waypoints \n", size);
+ i = 0;
+ waypoints_ready = true;
+ }
+ waiting_for_center_ = false;
+ input_.polygon.points.clear();
+ }else if(input_.polygon.points.empty()){
+ waypoints_ready = false;
+ //first control point, so initialize header of boundary polygon
+ input_.header = pointStampped->header;
+ input_.polygon.points.push_back(costmap_2d::toPoint32(pointStampped->point));
+ }else if(input_.header.frame_id != pointStampped->header.frame_id){
+ waypoints_ready = false;
+ ROS_ERROR("Frame mismatch, restarting polygon selection");
+ input_.polygon.points.clear();
+ }else if(input_.polygon.points.size() > 1 && pointsNearby(input_.polygon.points.front(), pointStampped->point,
+ average_distance*0.1)){
+ waypoints_ready = false;
+ //check if last boundary point, i.e. nearby to first point
+ if(input_.polygon.points.size() < 3){
+ ROS_ERROR("Not a valid polygon, restarting");
+ input_.polygon.points.clear();
+ }else{
+ waiting_for_center_ = true;
+ ROS_WARN("Please select an initial point for exploration inside the polygon");
+ }
+ }else{
+ waypoints_ready = false;
+ //otherwise, must be a regular point inside boundary polygon
+ input_.polygon.points.push_back(costmap_2d::toPoint32(pointStampped->point));
+ input_.header.stamp = ros::Time::now();
+ }
+}
+
+/**
+ * @brief Publish markers for visualization of points for boundary polygon.
+ */
+void WaypointPublisherNode::vizPubCallback(){
+
+ visualization_msgs::Marker points, line_strip;
+
+ points.header = line_strip.header = input_.header;
+ points.ns = line_strip.ns = "explore_points";
+
+ points.id = 0;
+ line_strip.id = 1;
+
+ points.type = visualization_msgs::Marker::SPHERE_LIST;
+ line_strip.type = visualization_msgs::Marker::LINE_STRIP;
+
+ if(!input_.polygon.points.empty()){
+
+ points.action = line_strip.action = visualization_msgs::Marker::ADD;
+ points.pose.orientation.w = line_strip.pose.orientation.w = 1.0;
+
+ points.scale.x = points.scale.y = 0.1;
+ line_strip.scale.x = 0.05;
+
+ BOOST_FOREACH(geometry_msgs::Point32 point, input_.polygon.points){
+ line_strip.points.push_back(costmap_2d::toPoint(point));
+ points.points.push_back(costmap_2d::toPoint(point));
+ }
+
+ if(waiting_for_center_){
+ line_strip.points.push_back(costmap_2d::toPoint(input_.polygon.points.front()));
+ points.color.a = points.color.r = line_strip.color.r = line_strip.color.a = 1.0;
+ }else{
+ points.color.a = points.color.b = line_strip.color.b = line_strip.color.a = 1.0;
+ }
+ }else{
+ points.action = line_strip.action = visualization_msgs::Marker::DELETE;
+ }
+ point_viz_pub_.publish(points);
+ point_viz_pub_.publish(line_strip);
+
+ }
+
+void WaypointPublisherNode::publishNextWayPoints(){
+ if(i < waypoints.size()){
+
+ const WaypointWithTime& wp = waypoints[i];
+
+ if(!published){
+
+ printf("Publishing #%d x=%f y=%f z=%f yaw=%f, and wait for %fs. \n", (int)i, wp.wp.position.x, wp.wp.position.y, wp.wp.position.z, wp.wp.yaw, wp.waiting_time);
+
+ published = 1;
+ start_time = ros::Time::now().toSec();
+ }
+
+ if((current_time-start_time) < wp.waiting_time){
+
+ //Rotate into BF
+ waypointBF = control_mode.rotateGFtoBF(wp.wp.position.x-current_gps_.pose.pose.position.x, wp.wp.position.y-current_gps_.pose.pose.position.y, wp.wp.position.z, 0, 0, gps_yaw);
+
+ desired_wp.position.x = (current_gps_.pose.pose.position.x + waypointBF(0));
+ desired_wp.position.y = (current_gps_.pose.pose.position.y + waypointBF(1));
+ desired_wp.position.z = wp.wp.position.z;
+ desired_wp.yaw = wp.wp.yaw;
+
+ desired_wp.jerk.x = 1;
+
+ desired_wp.header.stamp = ros::Time::now();
+ desired_wp.header.frame_id = "desired_mission_frame";
+ trajectory_pub.publish(desired_wp);
+
+ current_time = ros::Time::now().toSec();
+
+ }
+ else{
+
+ i = i + 1;
+ published = 0;
+
+ }
+
+ } else {
+ std::cout << "i: " << i << ", waypoints.size(): " << waypoints.size() << "\n";
+ }
+}
+
+
+void WaypointPublisherNode::readWayPointsFromFile()
+{
+ if(!waypoints_read){
+ waypoints = waypoint_utility.Read_waypoints(waypoints);
+
+ int size = waypoints.size();
+ printf("Start publishing #%d waypoints \n", size);
+
+ waypoints_read = 1;
+ }
+
+ publishNextWayPoints();
+}
+
+void WaypointPublisherNode::liveWayPointsFromRviz(){
+ //std::cout << "inside liveWayPointsFromRviz\n";
+ if(waypoints_ready) {
+ //std::cout << "Waypoints are ready!\n";
+ publishNextWayPoints();
+ } else {
+ std::cout << "Waypoints are not ready!\n";
+ }
+}
+
+
void WaypointPublisherNode::OdometryCallback(const nav_msgs::OdometryConstPtr& odometry_msg){
ROS_INFO_ONCE("Position_controller_node got first GPS message.");
@@ -168,55 +410,9 @@ void WaypointPublisherNode::OdometryCallback(const nav_msgs::OdometryConstPtr& o
if(control_mode.GetSwitchValue()){
ROS_INFO("Waypoint Mission Mode triggered");
+ //readWayPointsFromFile();
+ liveWayPointsFromRviz();
- if(!waypoints_read){
- waypoints = waypoint_utility.Read_waypoints(waypoints);
-
- int size = waypoints.size();
- printf("Start publishing #%d waypoints \n", size);
-
- waypoints_read = 1;
- }
-
- if(i < waypoints.size()){
-
- const WaypointWithTime& wp = waypoints[i];
-
- if(!published){
-
- printf("Publishing #%d x=%f y=%f z=%f yaw=%f, and wait for %fs. \n", (int)i, wp.wp.position.x, wp.wp.position.y, wp.wp.position.z, wp.wp.yaw, wp.waiting_time);
-
- published = 1;
- start_time = ros::Time::now().toSec();
- }
-
- if((current_time-start_time) < wp.waiting_time){
-
- //Rotate into BF
- waypointBF = control_mode.rotateGFtoBF(wp.wp.position.x-current_gps_.pose.pose.position.x, wp.wp.position.y-current_gps_.pose.pose.position.y, wp.wp.position.z, 0, 0, gps_yaw);
-
- desired_wp.position.x = (current_gps_.pose.pose.position.x + waypointBF(0));
- desired_wp.position.y = (current_gps_.pose.pose.position.y + waypointBF(1));
- desired_wp.position.z = wp.wp.position.z;
- desired_wp.yaw = wp.wp.yaw;
-
- desired_wp.jerk.x = 1;
-
- desired_wp.header.stamp = ros::Time::now();
- desired_wp.header.frame_id = "desired_mission_frame";
- trajectory_pub.publish(desired_wp);
-
- current_time = ros::Time::now().toSec();
-
- }
- else{
-
- i = i + 1;
- published = 0;
-
- }
-
- }
}
//Autonomous Mode triggered
else if(auto_mode.GetSwitchValue()){
@@ -229,7 +425,7 @@ void WaypointPublisherNode::OdometryCallback(const nav_msgs::OdometryConstPtr& o
desired_wp.position.z = desired_wp.position.z + cmd_vel.linear.z;
desired_wp.yaw = gps_yaw + cmd_vel.angular.z;
- desired_wp.jerk.x = 1; //Set flag for position controller
+ desired_wp.jerk.x = 1; //Set flag for position controller
desired_wp.header.stamp = ros::Time::now();
desired_wp.header.frame_id = "desired_auto_frame";
@@ -244,22 +440,22 @@ void WaypointPublisherNode::OdometryCallback(const nav_msgs::OdometryConstPtr& o
waypointBF = control_mode.rotateGFtoBF(threedNav_trajectory.position(0)-current_gps_.pose.pose.position.x, threedNav_trajectory.position(1)-current_gps_.pose.pose.position.y, threedNav_trajectory.position(2), 0, 0, gps_yaw);
if( (threedNav_trajectory.position(0)==0.0) && (threedNav_trajectory.position(1)==0.0) && (threedNav_trajectory.position(2)==0.0)){
- desired_wp.position.x = current_gps_.pose.pose.position.x;
- desired_wp.position.y = current_gps_.pose.pose.position.y;
- desired_wp.position.z = current_gps_.pose.pose.position.z;
- desired_wp.yaw = gps_yaw;
+ desired_wp.position.x = current_gps_.pose.pose.position.x;
+ desired_wp.position.y = current_gps_.pose.pose.position.y;
+ desired_wp.position.z = current_gps_.pose.pose.position.z;
+ desired_wp.yaw = gps_yaw;
}
else{
- //desired_wp.position.x = threedNav_trajectory.position(0);
- //desired_wp.position.y = threedNav_trajectory.position(1);
+ //desired_wp.position.x = threedNav_trajectory.position(0);
+ //desired_wp.position.y = threedNav_trajectory.position(1);
- desired_wp.position.x = (current_gps_.pose.pose.position.x + waypointBF(0));
- desired_wp.position.y = (current_gps_.pose.pose.position.y + waypointBF(1));
- desired_wp.position.z = threedNav_trajectory.position(2);
- desired_wp.yaw = threedNav_trajectory.yaw;
+ desired_wp.position.x = (current_gps_.pose.pose.position.x + waypointBF(0));
+ desired_wp.position.y = (current_gps_.pose.pose.position.y + waypointBF(1));
+ desired_wp.position.z = threedNav_trajectory.position(2);
+ desired_wp.yaw = threedNav_trajectory.yaw;
}
- desired_wp.jerk.x = 1; //Set flag for position controller
+ desired_wp.jerk.x = 1; //Set flag for position controller
desired_wp.header.stamp = ros::Time::now();
desired_wp.header.frame_id = "3dnav_mission_frame";
@@ -269,16 +465,16 @@ void WaypointPublisherNode::OdometryCallback(const nav_msgs::OdometryConstPtr& o
else{
//clear waypoints variable
ROS_INFO("RESET");
- waypoints.clear();
+ //waypoints.clear();
i = 0;
waypoints_read = 0;
published = 0;
//modes
- desired_wp.snap.x = command_trajectory.snap(0); // takeoff
- desired_wp.snap.y = command_trajectory.snap(1); // land
- desired_wp.jerk.x = command_trajectory.jerk(0); //enable GPS
- desired_wp.jerk.y = 0.0; // enable mission
+ desired_wp.snap.x = command_trajectory.snap(0); // takeoff
+ desired_wp.snap.y = command_trajectory.snap(1); // land
+ desired_wp.jerk.x = command_trajectory.jerk(0); //enable GPS
+ desired_wp.jerk.y = 0.0; // enable mission
desired_wp.header.stamp = ros::Time::now();
desired_wp.header.frame_id = "desired_waypoint_frame";
diff --git a/quad_control/src/nodes/waypoint_publisher_node.h b/quad_control/src/nodes/waypoint_publisher_node.h
index aafabfb..422cbc4 100644
--- a/quad_control/src/nodes/waypoint_publisher_node.h
+++ b/quad_control/src/nodes/waypoint_publisher_node.h
@@ -37,6 +37,10 @@
#include
#include
#include
+#include
+#include
+#include
+#include
#include
#include
#include
@@ -65,6 +69,7 @@ class WaypointWithTime {
const float DEG_2_RAD = M_PI / 180.0;
std::vector Read_waypoints(std::vector waypoints);
+ quad_control::WaypointWithTime PointToWaypointWithTime(const geometry_msgs::Point32& point);
};
class WaypointPublisherNode {
@@ -82,9 +87,17 @@ class WaypointPublisherNode {
ros::Subscriber odometry_sub_;
ros::Subscriber cmd_vel_sub_;
ros::Subscriber cmd_threednav_sub_;
+ ros::Subscriber live_waypoint_sub_;
//Publisher
ros::Publisher trajectory_pub;
+ ros::Publisher point_viz_pub_;
+ ros::WallTimer point_viz_timer_;
+
+ // Visualization
+ geometry_msgs::PolygonStamped input_;
+ bool waiting_for_center_;
+ bool waypoints_ready;
//Waypoint variables
planning_msgs::EigenWayPoint current_waypoint_;
@@ -118,6 +131,11 @@ class WaypointPublisherNode {
void OdometryCallback(const nav_msgs::OdometryConstPtr& odometry_msg);
void CommandVelCallback(const geometry_msgs::TwistConstPtr& command_velocity_msg);
void threedNavCallback(const mav_msgs::CommandTrajectoryConstPtr& threed_nav_msg);
+ void wayPointCallback(const geometry_msgs::PointStampedConstPtr& pointStampped);
+ void vizPubCallback();
+ void publishNextWayPoints();
+ void readWayPointsFromFile();
+ void liveWayPointsFromRviz();
};
diff --git a/quad_gazebo/launch/quad_joystick_gps_kinect_kitchen.launch b/quad_gazebo/launch/quad_joystick_gps_kinect_kitchen.launch
index 98a6b3c..1e47a6a 100644
--- a/quad_gazebo/launch/quad_joystick_gps_kinect_kitchen.launch
+++ b/quad_gazebo/launch/quad_joystick_gps_kinect_kitchen.launch
@@ -63,7 +63,7 @@
-
+