From 143b0e9b7d2616ffc59b3479dc4fb4541ec0fe30 Mon Sep 17 00:00:00 2001 From: glc12125 Date: Sun, 7 Oct 2018 22:28:36 +0100 Subject: [PATCH 1/2] subscription done between quad_control and rviz clicked_point --- .../src/nodes/waypoint_publisher_node.cpp | 78 ++++++++++--------- .../src/nodes/waypoint_publisher_node.h | 3 + .../quad_joystick_gps_kinect_kitchen.launch | 2 +- 3 files changed, 46 insertions(+), 37 deletions(-) diff --git a/quad_control/src/nodes/waypoint_publisher_node.cpp b/quad_control/src/nodes/waypoint_publisher_node.cpp index 799a545..7b3583b 100644 --- a/quad_control/src/nodes/waypoint_publisher_node.cpp +++ b/quad_control/src/nodes/waypoint_publisher_node.cpp @@ -24,7 +24,7 @@ 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()) { @@ -56,6 +56,7 @@ 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); @@ -116,6 +117,11 @@ void WaypointPublisherNode::threedNavCallback(const mav_msgs::CommandTrajectoryC } +void WaypointPublisherNode::wayPointCallback(const geometry_msgs::PointStampedConstPtr& pointStampped) +{ + printf("I received point: (%f, %f, %f)", pointStampped->point.x, pointStampped->point.y, pointStampped->point.z); +} + void WaypointPublisherNode::OdometryCallback(const nav_msgs::OdometryConstPtr& odometry_msg){ ROS_INFO_ONCE("Position_controller_node got first GPS message."); @@ -176,26 +182,26 @@ void WaypointPublisherNode::OdometryCallback(const nav_msgs::OdometryConstPtr& o printf("Start publishing #%d waypoints \n", size); waypoints_read = 1; - } + } - if(i < waypoints.size()){ + if(i < waypoints.size()){ - const WaypointWithTime& wp = waypoints[i]; + const WaypointWithTime& wp = waypoints[i]; - if(!published){ + 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); + 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(); - } + published = 1; + start_time = ros::Time::now().toSec(); + } - if((current_time-start_time) < wp.waiting_time){ + if((current_time-start_time) < wp.waiting_time){ - //Rotate into BF + //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.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; @@ -206,17 +212,17 @@ void WaypointPublisherNode::OdometryCallback(const nav_msgs::OdometryConstPtr& o desired_wp.header.frame_id = "desired_mission_frame"; trajectory_pub.publish(desired_wp); - current_time = ros::Time::now().toSec(); + current_time = ros::Time::now().toSec(); - } - else{ - - i = i + 1; - published = 0; + } + else{ + + i = i + 1; + published = 0; - } + } - } + } } //Autonomous Mode triggered else if(auto_mode.GetSwitchValue()){ @@ -229,7 +235,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 +250,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"; @@ -275,10 +281,10 @@ void WaypointPublisherNode::OdometryCallback(const nav_msgs::OdometryConstPtr& o 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..912ded2 100644 --- a/quad_control/src/nodes/waypoint_publisher_node.h +++ b/quad_control/src/nodes/waypoint_publisher_node.h @@ -37,6 +37,7 @@ #include #include #include +#include #include #include #include @@ -82,6 +83,7 @@ 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; @@ -118,6 +120,7 @@ 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); }; 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 @@ - + From 8d4f6ec1f8068f2fd7f5776d6bd2e4fcdc020c8c Mon Sep 17 00:00:00 2001 From: glc12125 Date: Mon, 8 Oct 2018 17:13:38 +0100 Subject: [PATCH 2/2] get live waypoints from rviz GUI --- quad_control/CMakeLists.txt | 11 +- quad_control/package.xml | 2 + .../src/nodes/waypoint_publisher_node.cpp | 290 +++++++++++++++--- .../src/nodes/waypoint_publisher_node.h | 15 + 4 files changed, 263 insertions(+), 55 deletions(-) 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 7b3583b..0e8a59d 100644 --- a/quad_control/src/nodes/waypoint_publisher_node.cpp +++ b/quad_control/src/nodes/waypoint_publisher_node.cpp @@ -19,6 +19,10 @@ */ #include "waypoint_publisher_node.h" +#include +#include +#include +#include namespace quad_control { @@ -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(); @@ -60,6 +128,8 @@ WaypointPublisherNode::WaypointPublisherNode(){ //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."); @@ -87,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."); } @@ -119,9 +194,170 @@ void WaypointPublisherNode::threedNavCallback(const mav_msgs::CommandTrajectoryC void WaypointPublisherNode::wayPointCallback(const geometry_msgs::PointStampedConstPtr& pointStampped) { - printf("I received point: (%f, %f, %f)", pointStampped->point.x, pointStampped->point.y, pointStampped->point.z); + 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."); @@ -174,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()){ @@ -275,7 +465,7 @@ 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; diff --git a/quad_control/src/nodes/waypoint_publisher_node.h b/quad_control/src/nodes/waypoint_publisher_node.h index 912ded2..422cbc4 100644 --- a/quad_control/src/nodes/waypoint_publisher_node.h +++ b/quad_control/src/nodes/waypoint_publisher_node.h @@ -37,7 +37,10 @@ #include #include #include +#include #include +#include +#include #include #include #include @@ -66,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 { @@ -87,6 +91,13 @@ class WaypointPublisherNode { //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_; @@ -121,6 +132,10 @@ class WaypointPublisherNode { 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(); };