Merge pull request #38 from rosflight/34-waypoint-visualization

34 waypoint visualization

rosplane/params/fixedwing_mission.yaml

@@ -5,17 +5,17 @@ wp:
   use_chi: False
   va_d: 25.0
 wp:
-  w: [2000.0, 1000.0, -50.0]
+  w: [1000.0, 400.0, -50.0]
   chi_d: 1.1518
   use_chi: False
   va_d: 25.0
 wp:
-  w: [2000.0, -1000.0, -50.0]
+  w: [1000.0, -500.0, -50.0]
   chi_d: 1.1518
   use_chi: False
   va_d: 25.0
 wp:
-  w: [0.0, -1000.0, -50.0]
+  w: [0.0, -800.0, -50.0]
   chi_d: 1.1518
   use_chi: False
   va_d: 25.0

rosplane/src/path_planner.cpp

@@ -43,8 +43,10 @@ class path_planner : public rclcpp::Node
   bool update_path(const rosplane_msgs::srv::AddWaypoint::Request::SharedPtr & req,
                    const rosplane_msgs::srv::AddWaypoint::Response::SharedPtr & res);
 
-  bool clear_path(const std_srvs::srv::Trigger::Request::SharedPtr & req,
+  bool clear_path_callback(const std_srvs::srv::Trigger::Request::SharedPtr & req,
                   const std_srvs::srv::Trigger::Response::SharedPtr & res);
+
+  void clear_path();
 
   bool print_path(const std_srvs::srv::Trigger::Request::SharedPtr & req,
                   const std_srvs::srv::Trigger::Response::SharedPtr & res);
@@ -75,7 +77,9 @@ path_planner::path_planner()
     : Node("path_planner"), params(this)
 {
 
-  waypoint_publisher = this->create_publisher<rosplane_msgs::msg::Waypoint>("waypoint_path", 10);
+  rclcpp::QoS qos_transient_local_10_(10);
+  qos_transient_local_10_.transient_local();
+  waypoint_publisher = this->create_publisher<rosplane_msgs::msg::Waypoint>("waypoint_path", qos_transient_local_10_);
 
   next_waypoint_service = this->create_service<std_srvs::srv::Trigger>(
     "publish_next_waypoint",
@@ -87,7 +91,7 @@ path_planner::path_planner()
 
   clear_waypoint_service = this->create_service<std_srvs::srv::Trigger>(
     "clear_waypoints",
-    std::bind(&path_planner::clear_path, this, _1, _2));
+    std::bind(&path_planner::clear_path_callback, this, _1, _2));
 
   print_waypoint_service = this->create_service<std_srvs::srv::Trigger>(
     "print_waypoints",
@@ -106,6 +110,10 @@ path_planner::path_planner()
   timer_ = this->create_wall_timer(1000ms, std::bind(&path_planner::timer_callback, this));
 
   num_waypoints_published = 0;
+
+  // Initialize by publishing a clear path command.
+  // This makes sure rviz doesn't show stale waypoints if rosplane is restarted.
+  clear_path();
 }
 
 path_planner::~path_planner() {}
@@ -177,17 +185,23 @@ bool path_planner::update_path(const rosplane_msgs::srv::AddWaypoint::Request::S
   return true;
 }
 
-bool path_planner::clear_path(const std_srvs::srv::Trigger::Request::SharedPtr & req,
+bool path_planner::clear_path_callback(const std_srvs::srv::Trigger::Request::SharedPtr & req,
                               const std_srvs::srv::Trigger::Response::SharedPtr & res) {
+  clear_path();
+
+  res->success = true;
+  return true;
+}
+
+void path_planner::clear_path() {
   wps.clear();
 
   rosplane_msgs::msg::Waypoint new_waypoint;
   new_waypoint.clear_wp_list = true;
 
   waypoint_publisher->publish(new_waypoint);
 
-  res->success = true;
-  return true;
+  num_waypoints_published = 0;
 }
 
 bool path_planner::print_path(const std_srvs::srv::Trigger::Request::SharedPtr & req,
@@ -215,7 +229,7 @@ bool path_planner::print_path(const std_srvs::srv::Trigger::Request::SharedPtr &
 
 bool path_planner::load_mission(const rosflight_msgs::srv::ParamFile::Request::SharedPtr & req,
                                 const rosflight_msgs::srv::ParamFile::Response::SharedPtr & res) {
-  // std::string filename = req->filename;
+  clear_path();
   res->success = load_mission_from_file(req->filename);
   return true;
 }

rosplane_gcs/CMakeLists.txt

@@ -0,0 +1,54 @@
+cmake_minimum_required(VERSION 3.8)
+project(rosplane_gcs)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(NOT CMAKE_BUILD_TYPE)
+  set(CMAKE_BUILD_TYPE "Release")
+endif(NOT CMAKE_BUILD_TYPE)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rosplane_msgs REQUIRED)
+find_package(visualization_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(geometry_msgs REQUIRED)
+
+ament_export_dependencies(rclcpp visualization_msgs)
+
+install(DIRECTORY launch config resource DESTINATION share/${PROJECT_NAME})
+
+# Publisher node executable
+add_executable(rviz_waypoint_publisher
+               src/rviz_waypoint_publisher.cpp)
+ament_target_dependencies(rviz_waypoint_publisher rosplane_msgs visualization_msgs rclcpp tf2 tf2_ros geometry_msgs)
+install(TARGETS 
+  rviz_waypoint_publisher
+  DESTINATION lib/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()