Refactor code

- Remove custom msg. - Update launch files. - Split into seperate ROS nodes.
quantumxt · Jun 26, 2022 · d7b17d6 · d7b17d6
1 parent 3d8ea0b
commit d7b17d6
Show file tree

Hide file tree

Showing 13 changed files with 213 additions and 258 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -14,34 +14,21 @@ roscpp
 tf
 roslib
 actionlib
+std_msgs
 geometry_msgs
 move_base_msgs
 nav_msgs
 visualization_msgs
 message_generation
 )
 
-add_message_files(
-FILES
-wpg_stat.msg
-)
-
-## Generate services in the 'srv' folder
 add_service_files(
   FILES
   Trigger.srv
 )
 
-## Generate actions in the 'action' folder
-#add_action_files(
-#DIRECTORY action
-#   FILES wpServer.action
-#)
-
 ## Generate added messages and services with any dependencies listed here
-generate_messages(
-DEPENDENCIES actionlib_msgs std_msgs
-)
+generate_messages()
 
 ###################################
 ## catkin specific configuration ##
@@ -55,7 +42,7 @@ DEPENDENCIES actionlib_msgs std_msgs
 
 catkin_package(
     INCLUDE_DIRS include
-#  LIBRARIES waypointgen
+    LIBRARIES wpg_utils
     CATKIN_DEPENDS interactive_markers roscpp tf visualization_msgs actionlib_msgs message_runtime
 #  DEPENDS system_lib
 )
@@ -70,27 +57,26 @@ add_library(wpg_utils src/waypointgen_utils.cpp)
 target_link_libraries(wpg_utils ${catkin_LIBRARIES})
 add_dependencies(wpg_utils ${wpg_utils_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
-add_executable(setpoint_marker src/setpoint_marker.cpp)
-target_link_libraries(
-    setpoint_marker
-    ${catkin_LIBRARIES} 
-    yaml-cpp
-    wpg_utils
-)
+add_executable(setpoint_marker src/setpoint_marker.cpp src/setpoint_marker_node.cpp)
+add_executable(setpoint_server src/setpoint_server.cpp src/setpoint_server_node.cpp)
+
 add_dependencies(
     setpoint_marker ${${PROJECT_NAME}_EXPORTED_TARGETS}  ${catkin_EXPORTED_TARGETS}
 )
+add_dependencies(
+    setpoint_server ${${PROJECT_NAME}_EXPORTED_TARGETS}  ${catkin_EXPORTED_TARGETS}
+)
 
-add_executable(setpoint_server src/setpoint_server.cpp)
 target_link_libraries(
-    setpoint_server
-    ${catkin_LIBRARIES}
-    yaml-cpp
+    setpoint_marker yaml-cpp wpg_utils ${catkin_LIBRARIES} 
 )
-add_dependencies(
-    setpoint_server ${${PROJECT_NAME}_EXPORTED_TARGETS}  ${catkin_EXPORTED_TARGETS}
+
+
+target_link_libraries(
+    setpoint_server yaml-cpp wpg_utils ${catkin_LIBRARIES}
 )
 
+
 ## Mark executables and/or libraries for installation
 install(
     TARGETS wpg_utils

diff --git a/README.md b/README.md
@@ -1,6 +1,8 @@
 # waypointgen
 
-[![ROS CI](https://github.com/1487quantum/waypointgen/actions/workflows/ros_ci.yml/badge.svg?branch=master)](https://github.com/1487quantum/waypointgen/actions/workflows/ros_ci.yml)
+[![ROS CI](https://img.shields.io/github/workflow/status/1487quantum/waypointgen/ROS%20CI?label=CI&logo=ros&style=for-the-badge)](https://github.com/1487quantum/waypointgen/actions/workflows/ros_ci.yml)
+
+
 
 A waypoint generator (using InteractiveMarkers in Rviz) and a waypoint server for waypoint playback. The waypoints are saved as a **YAML** file.
 
@@ -89,3 +91,11 @@ rosservice call /trigger_play 3
 - Refactored code.
 - Replace ros topic subscription to ros service to trigger playback.
 - Add ROS CI.
+
+### v0.1.2
+
+- Add benchmark metrics.
+- Split into seperate ROS nodes.
+- Refactored code.
+- Removed custom msg.
+- Update launch files.
diff --git a/include/waypointgen/setpoint_marker.h b/include/waypointgen/setpoint_marker.h
@@ -1,27 +1,27 @@
+#include <boost/bind.hpp>
+#include <map>
+#include <ctime>
+#include <filesystem>
+#include <fstream>
+#include <yaml-cpp/yaml.h>
 
-#include <ros/package.h>
 #include <ros/ros.h>
+#include <ros/package.h>
 
-#include <geometry_msgs/Point.h>
-#include <geometry_msgs/PoseWithCovariance.h>
-#include <std_msgs/String.h>
 #include <tf/tf.h>
 #include <tf/transform_datatypes.h>
 
-#include <interactive_markers/interactive_marker_server.h>
-#include <interactive_markers/menu_handler.h>
+#include <std_msgs/String.h>
+#include <geometry_msgs/Point.h>
+#include <geometry_msgs/PoseWithCovariance.h>
 #include <visualization_msgs/InteractiveMarkerInit.h>
 
-#include <boost/bind.hpp>
-#include <ctime>
-#include <filesystem>
-#include <fstream>
-#include <map>
-#include <yaml-cpp/yaml.h>
+#include <interactive_markers/interactive_marker_server.h>
+#include <interactive_markers/menu_handler.h>
 
 #include "waypointgen/waypointgen_utils.h"
 
-typedef std::map<std::string, geometry_msgs::PoseWithCovariance> p_map;
+using p_map = std::map<std::string, geometry_msgs::PoseWithCovariance>;
 
 class waypointgen_marker {
 public:

diff --git a/include/waypointgen/setpoint_server.h b/include/waypointgen/setpoint_server.h
@@ -1,30 +1,37 @@
+#include <chrono>
+#include <filesystem>
+#include <fstream>
+#include <thread>
+#include <vector>
+#include <string>
+#include <yaml-cpp/yaml.h>
+
 #include <ros/ros.h>
 
-#include <actionlib/client/simple_action_client.h>
-#include <actionlib/client/simple_client_goal_state.h>
-#include <string>
 #include <tf/tf.h>
-
+#include <std_msgs/Float32.h>
+#include <geometry_msgs/Point.h>
 #include <geometry_msgs/PoseArray.h>
 #include <geometry_msgs/PoseStamped.h>
+
+#include <nav_msgs/Path.h>
 #include <move_base_msgs/MoveBaseAction.h>
 #include <move_base_msgs/MoveBaseFeedback.h>
 #include <move_base_msgs/MoveBaseGoal.h>
 #include <move_base_msgs/MoveBaseResult.h>
-#include <nav_msgs/Path.h>
-#include <std_msgs/Float32.h>
 
-#include <thread>
-#include <chrono>
-#include <filesystem>
-#include <fstream>
-#include <yaml-cpp/yaml.h>
+#include <actionlib/client/simple_action_client.h>
+#include <actionlib/client/simple_client_goal_state.h>
 
+#include "waypointgen/waypointgen_utils.h"
 #include "waypointgen/Trigger.h"
-#include "waypointgen/wpg_stat.h"
 
-#define DEBUG 1
-#define PI 3.1415926535897932385
+constexpr bool DEBUG = 1;
+
+using MoveBaseClient =
+    actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction>;
+using clock_type = std::chrono::steady_clock;
+using d_type = std::chrono::duration<double, std::ratio<1>>;
 
 class waypointgen_server {
 public:
@@ -33,6 +40,7 @@ class waypointgen_server {
   ~waypointgen_server(void) {}
 
   int init();
+  inline ros::NodeHandle *get_nh() { return &this->nh_; };
 
   // Status of server
   /*
@@ -44,11 +52,14 @@ class waypointgen_server {
   */
   enum class ServerState { PLAY, STOP, PAUSE, IDLE };
 
+  void setGlobalPathTopic(const ros::NodeHandle &n,
+                          const std::string &param_name,
+                          std::string &target_topic);
+
   // Callbacks
   bool start_p2p(waypointgen::Trigger::Request &req,
                  waypointgen::Trigger::Response &res);
   void gPlanCallback(const nav_msgs::Path &msg);
-  // void wpgStatCallback(const waypointgen::wpg_stat &msg);
   void timerGoalCallback(const ros::TimerEvent &event);
 
   // Move base action callback
@@ -59,13 +70,10 @@ class waypointgen_server {
   // Waypoints
   int loadWaypointList(const std::string &list_path);
   float getPathDist(const std::vector<geometry_msgs::PoseStamped> &pathVector);
-  geometry_msgs::PoseStamped
-  convertToPoseStamped(const std::string &poseFrameID,
-                       const geometry_msgs::Pose &poseTarget);
+  float getPathDist(const geometry_msgs::Point &pointStart,
+                    const geometry_msgs::Point &pointEnd);
 
-  inline void set_waypointcount(const int &num_wp) {
-    this->numOfWaypoints = num_wp;
-  };
+  inline void set_waypointcount(const int &num_wp) { this->wp_count = num_wp; };
 
   inline int get_s_state_delay() { return s_state_delay; }
   inline void set_s_state_delay(const int &sdelay) {
@@ -75,8 +83,6 @@ class waypointgen_server {
   inline ServerState *get_state() { return &this->current_state; };
   inline void set_state(const ServerState &ss) { this->current_state = ss; };
 
-  inline ros::NodeHandle *get_nh() { return &this->nh_; };
-
   inline std::vector<geometry_msgs::Pose> *get_wpList() {
     return &this->wpList;
   }
@@ -98,9 +104,10 @@ class waypointgen_server {
            const geometry_msgs::Pose &qpt); // Point 2 Point
   void begin_playback();
 
+  waypointgen_utils wpg_utils;
+
 private:
-  typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction>
-      MoveBaseClient;
+  ros::NodeHandle nh_;
 
   // Service
   ros::ServiceServer trigger_server;
@@ -114,15 +121,16 @@ class waypointgen_server {
   ros::Publisher pointPubGoal;  // publish waypoint_goal
   ros::Publisher distToGoalPub; // publish distance to goal
 
-  ros::NodeHandle nh_;
-
   std::string global_path_topic;
 
-  int numOfWaypoints;
-  float distToGoal;
+  ServerState current_state;
+  int s_state_delay; // Delay before starting the server play, in seconds
 
-  // Total number of waypoints
-  int wp_count;
+  std::vector<geometry_msgs::Pose> wpList; // Waypoint listed
+  geometry_msgs::Pose currentWaypoint;     // current waypoint
+
+  int wp_count; // Total number of waypoints
+  float distToGoal;
 
   // Distance to goal
   ros::Timer timerGoal; // Refresh and get distance to goal
@@ -131,19 +139,7 @@ class waypointgen_server {
   float ecDistMax;      // Euclidean distance from goal, Max
   float gpDistMax;      // Global Path distance from goal, Max
 
-  ServerState current_state;
-  int s_state_delay; // Delay before starting the server play, in
-                     // seconds
-
-  std::vector<geometry_msgs::Pose> wpList; // Waypoint listed
-  geometry_msgs::Pose currentWaypoint;     // current waypoint
-
-  // Timer for benchmarking
-  using clock_type = std::chrono::steady_clock;
-  using d_type = std::chrono::duration<double, std::ratio<1>>;
-
-  std::chrono::time_point<clock_type> path_timer{clock_type::now()};
-
-  //Benchmark vals
-  std::vector<bool> wpt_benchmark_success;
+  std::chrono::time_point<clock_type> path_timer{
+      clock_type::now()};                  // Timer for benchmarking
+  std::vector<bool> wpt_benchmark_success; // Benchmark vals
 };
diff --git a/include/waypointgen/waypointgen_utils.h b/include/waypointgen/waypointgen_utils.h
@@ -1,22 +1,29 @@
 
 #include <geometry_msgs/Point.h>
 #include <geometry_msgs/PoseWithCovariance.h>
+#include <std_msgs/Header.h>
 #include <std_msgs/String.h>
 #include <tf/tf.h>
 
 class waypointgen_utils {
 
 public:
   waypointgen_utils();
+  void update_header(std_msgs::Header &hd, const std::string &frameID);
+  template <typename T>
+  inline void add_timestamp(T &targetMsg, const std::string frameID,
+                            const geometry_msgs::Pose &initPose) {
+    update_header(targetMsg.header, frameID);
+    targetMsg.pose = initPose; // set pose
+  };
 
   geometry_msgs::PoseWithCovariance addPoseCov(const geometry_msgs::Point &pt,
                                                const tf::Quaternion &q_rotate);
-  void set_pose_position(geometry_msgs::Point &from_point,
-                         const geometry_msgs::Point &to_point);
 
   std::string getCurrentTime();
 
   void printDebugPose(const std::string &dmsg, const std::string &wp_name,
                       const geometry_msgs::PoseWithCovariance &pw);
 
+  static constexpr double PI = 3.1415926535897932385;
 };
diff --git a/launch/setpoint_marker.launch b/launch/setpoint_marker.launch
@@ -1,4 +1,8 @@
 <launch>
-  <node pkg="waypointgen" type="setpoint_marker" respawn="false" name="setpoint_marker" output="screen"/>
-  <node pkg="rviz" type="rviz" respawn="false" name="rviz"/>
-</launch>
+  <arg name="show_rviz" default="true" />
+
+  <node pkg="waypointgen" type="setpoint_marker" respawn="false" name="setpoint_marker" output="screen" />
+  <group if="$(arg show_rviz)">
+    <node pkg="rviz" type="rviz" respawn="false" name="rviz" args="-d $(find waypointgen)/rviz/wp_markers.rviz"/>
+  </group>
+</launch>
diff --git a/msg/wpg_stat.msg b/msg/wpg_stat.msg
diff --git a/package.xml b/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>waypointgen</name>
-  <version>0.1.1</version>
+  <version>0.1.2</version>
   <description>The waypointgen package</description>
 
   <maintainer email="[email protected]">1487quantum</maintainer>
@@ -14,6 +14,7 @@
   <build_depend>tf</build_depend>
   <build_depend>visualization_msgs</build_depend>
   <build_depend>roslib</build_depend>
+  <build_depend>std_msgs</build_depend>
   <build_depend>actionlib_msgs</build_depend>
   <build_depend>geometry_msgs</build_depend>
   <build_depend>move_base_msgs</build_depend>
@@ -30,6 +31,7 @@
   <exec_depend>tf</exec_depend>
   <exec_depend>visualization_msgs</exec_depend>
   <exec_depend>message_runtime</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
   <exec_depend>actionlib_msgs</exec_depend>
   <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>move_base_msgs</exec_depend>