Separate out executable while simplifying global_planner library (#452)

* Simplify library

* Separate global_planner_node executable

* Fix code style

global_planner/CMakeLists.txt

@@ -122,7 +122,6 @@ generate_dynamic_reconfigure_options(
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
   INCLUDE_DIRS include
-  LIBRARIES global_planner cell node
   CATKIN_DEPENDS roscpp rospy std_msgs geometry_msgs sensor_msgs message_runtime tf
 #  DEPENDS system_lib
 )
@@ -150,14 +149,11 @@ include_directories(
 link_libraries(${OCTOMAP_LIBRARIES})
 
 ## Declare a C++ library
-add_library(cell
-  src/library/cell.cpp
-)
-add_library(node
-  src/library/node.cpp
-)
 add_library(global_planner
+  src/library/node.cpp
+  src/library/cell.cpp
   src/library/global_planner.cpp
+  src/nodes/global_planner_node.cpp
 )
 
 ## Add cmake target dependencies of the library
@@ -166,11 +162,11 @@ add_library(global_planner
 add_dependencies(global_planner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 ## Declare a C++ executable
-add_executable(global_planner_node src/nodes/global_planner_node.cpp)
+add_executable(global_planner_node src/nodes/global_planner_node_main.cpp)
 
 ## Specify libraries to link a library or executable target against
 target_link_libraries(global_planner_node
-  global_planner cell node ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${YAML_CPP_LIBRARIES}
+  global_planner ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${YAML_CPP_LIBRARIES}
 )
 
 #############

global_planner/include/global_planner/cell.h

@@ -11,7 +11,7 @@
 
 namespace global_planner {
 
-double CELL_SCALE = 1.0;
+static double CELL_SCALE = 1.0;
 
 class Cell {
  public:

global_planner/include/global_planner/common.h

@@ -15,7 +15,7 @@ double squared(T x) {
 
 // Returns a weighted average of start and end, where ratio is the weight of
 // start
-double interpolate(double start, double end, double ratio) { return start + (end - start) * ratio; }
+inline double interpolate(double start, double end, double ratio) { return start + (end - start) * ratio; }
 
 // Returns Map[key] if it exists, default_val otherwise
 template <typename Key, typename Value, typename Map>
@@ -92,19 +92,19 @@ double distance(const P& p1, const P& p2) {
   return norm((p2.x - p1.x), (p2.y - p1.y), (p2.z - p1.z));
 }
 
-double clocksToMicroSec(std::clock_t start, std::clock_t end) {
+inline double clocksToMicroSec(std::clock_t start, std::clock_t end) {
   return (end - start) / (double)(CLOCKS_PER_SEC / 1000000);
 }
 
 // returns angle in the range [-pi, pi]
-double angleToRange(double angle) {
+inline double angleToRange(double angle) {
   angle += M_PI;
   angle -= (2 * M_PI) * std::floor(angle / (2 * M_PI));
   angle -= M_PI;
   return angle;
 }
 
-double posterior(double p, double prior) {
+inline double posterior(double p, double prior) {
   // p and prior are independent measurements of the same event
   double prob_obstacle = p * prior;
   double prob_free = (1 - p) * (1 - prior);

global_planner/include/global_planner/common_ros.h

@@ -23,12 +23,13 @@ tf::Vector3 toTfVector3(const P& point) {
   return tf::Vector3(point.x, point.y, point.z);
 }
 
-double distance(const geometry_msgs::PoseStamped& a, const geometry_msgs::PoseStamped& b) {
+inline double distance(const geometry_msgs::PoseStamped& a, const geometry_msgs::PoseStamped& b) {
   return distance(a.pose.position, b.pose.position);
 }
 
-geometry_msgs::TwistStamped transformTwistMsg(const tf::TransformListener& listener, const std::string& target_frame,
-                                              const std::string& fixed_frame, const geometry_msgs::TwistStamped& msg) {
+inline geometry_msgs::TwistStamped transformTwistMsg(const tf::TransformListener& listener,
+                                                     const std::string& target_frame, const std::string& fixed_frame,
+                                                     const geometry_msgs::TwistStamped& msg) {
   auto transformed_msg = msg;
   geometry_msgs::Vector3Stamped before;
   before.vector = msg.twist.linear;
@@ -40,7 +41,7 @@ geometry_msgs::TwistStamped transformTwistMsg(const tf::TransformListener& liste
 }
 
 // Returns a spectral color between red (0.0) and blue (1.0)
-std_msgs::ColorRGBA spectralColor(double hue, double alpha = 1.0) {
+inline std_msgs::ColorRGBA spectralColor(double hue, double alpha = 1.0) {
   std_msgs::ColorRGBA color;
   color.r = std::max(0.0, 2 * hue - 1);
   color.g = 1.0 - 2.0 * std::abs(hue - 0.5);
@@ -72,12 +73,12 @@ visualization_msgs::Marker createMarker(int id, Point position, Color color, dou
 // }
 
 // Returns true if msg1 and msg2 have both the same altitude and orientation
-bool hasSameYawAndAltitude(const geometry_msgs::Pose& msg1, const geometry_msgs::Pose& msg2) {
+inline bool hasSameYawAndAltitude(const geometry_msgs::Pose& msg1, const geometry_msgs::Pose& msg2) {
   return msg1.orientation.z == msg2.orientation.z && msg1.orientation.w == msg2.orientation.w &&
          msg1.position.z == msg2.position.z;
 }
 
-double pathLength(const nav_msgs::Path& path) {
+inline double pathLength(const nav_msgs::Path& path) {
   double total_dist = 0.0;
   for (int i = 1; i < path.poses.size(); ++i) {
     total_dist += distance(path.poses[i - 1], path.poses[i]);
@@ -86,7 +87,7 @@ double pathLength(const nav_msgs::Path& path) {
 }
 
 // Returns a path with only the corner points of msg
-std::vector<geometry_msgs::PoseStamped> filterPathCorners(const std::vector<geometry_msgs::PoseStamped>& msg) {
+inline std::vector<geometry_msgs::PoseStamped> filterPathCorners(const std::vector<geometry_msgs::PoseStamped>& msg) {
   std::vector<geometry_msgs::PoseStamped> corners = msg;
   corners.clear();
   if (msg.size() < 1) {
@@ -110,7 +111,7 @@ std::vector<geometry_msgs::PoseStamped> filterPathCorners(const std::vector<geom
   return corners;
 }
 
-double pathKineticEnergy(const nav_msgs::Path& path) {
+inline double pathKineticEnergy(const nav_msgs::Path& path) {
   if (path.poses.size() < 3) {
     return 0.0;
   }
@@ -132,7 +133,7 @@ double pathKineticEnergy(const nav_msgs::Path& path) {
   return total_energy;
 }
 
-double pathEnergy(const nav_msgs::Path& path, double up_penalty) {
+inline double pathEnergy(const nav_msgs::Path& path, double up_penalty) {
   double total_energy = 0.0;
   for (int i = 1; i < path.poses.size(); ++i) {
     total_energy += distance(path.poses[i - 1], path.poses[i]);

global_planner/include/global_planner/global_planner.h

@@ -20,7 +20,6 @@
 #include <global_planner/GlobalPlannerNodeConfig.h>
 #include <global_planner/PathWithRiskMsg.h>
 #include "global_planner/analysis.h"
-#include "global_planner/bezier.h"
 #include "global_planner/cell.h"
 #include "global_planner/common.h"
 #include "global_planner/common_ros.h"

global_planner/include/global_planner/global_planner_node.h

@@ -29,14 +29,7 @@
 #include <octomap_msgs/conversions.h>
 
 #include "avoidance/avoidance_node.h"
-#include "global_planner/PathWithRiskMsg.h"
-#include "global_planner/analysis.h"
-#include "global_planner/cell.h"
-#include "global_planner/common.h"
-#include "global_planner/common_ros.h"
 #include "global_planner/global_planner.h"
-#include "global_planner/search_tools.h"
-#include "global_planner/visitor.h"
 
 #ifndef DISABLE_SIMULATION
 #include <avoidance/rviz_world_loader.h>

global_planner/include/global_planner/node.h

@@ -31,12 +31,12 @@ class Node {
   Cell parent_;
 };
 
-bool operator==(const Node& lhs, const Node& rhs) { return lhs.isEqual(rhs); }
-bool operator<(const Node& lhs, const Node& rhs) { return lhs.isSmaller(rhs); }
-bool operator!=(const Node& lhs, const Node& rhs) { return !operator==(lhs, rhs); }
-bool operator>(const Node& lhs, const Node& rhs) { return operator<(rhs, lhs); }
-bool operator<=(const Node& lhs, const Node& rhs) { return !operator>(lhs, rhs); }
-bool operator>=(const Node& lhs, const Node& rhs) { return !operator<(lhs, rhs); }
+inline bool operator==(const Node& lhs, const Node& rhs) { return lhs.isEqual(rhs); }
+inline bool operator<(const Node& lhs, const Node& rhs) { return lhs.isSmaller(rhs); }
+inline bool operator!=(const Node& lhs, const Node& rhs) { return !operator==(lhs, rhs); }
+inline bool operator>(const Node& lhs, const Node& rhs) { return operator<(rhs, lhs); }
+inline bool operator<=(const Node& lhs, const Node& rhs) { return !operator>(lhs, rhs); }
+inline bool operator>=(const Node& lhs, const Node& rhs) { return !operator<(lhs, rhs); }
 
 typedef std::shared_ptr<Node> NodePtr;
 typedef std::pair<Node, double> NodeDistancePair;
@@ -67,7 +67,7 @@ class NodeWithoutSmooth : public Node {
   double getRotation(const Node& other) const { return 0.0; }
 };
 
-double SPEEDNODE_RADIUS = 5.0;
+static double SPEEDNODE_RADIUS = 5.0;
 // Node represents 3D position, orientation and speed
 // TODO: Needs to check the risk of Cells between cell and parent
 class SpeedNode : public Node {

global_planner/include/global_planner/search_tools.h

@@ -29,15 +29,15 @@ struct SearchInfo {
   double search_time;  // in micro seconds
 };
 
-void printSearchInfo(SearchInfo info, std::string node_type = "Node", double overestimate_factor = 1.0) {
+inline void printSearchInfo(SearchInfo info, std::string node_type = "Node", double overestimate_factor = 1.0) {
   double avg_time = info.search_time / info.num_iter;
   std::cout << std::setw(20) << std::left << node_type << std::setw(10) << std::setprecision(3) << avg_time
             << std::setw(10) << std::setprecision(3) << overestimate_factor << std::setw(10) << info.num_iter
             << std::setw(10) << 0.0;
 }
 
 // Returns a path where corners are smoothed with quadratic Bezier-curves
-nav_msgs::Path smoothPath(const nav_msgs::Path& path) {
+inline nav_msgs::Path smoothPath(const nav_msgs::Path& path) {
   if (path.poses.size() < 3) {
     return path;
   }
@@ -118,16 +118,16 @@ std::vector<Cell> simplifyPath(GlobalPlanner* global_planner, std::vector<Cell>&
 }
 
 template <typename GlobalPlanner>
-SearchInfo findSmoothPath(GlobalPlanner* global_planner, std::vector<Cell>& path, const NodePtr& s, const GoalCell& t,
-                          int max_iterations = 2000) {
+inline SearchInfo findSmoothPath(GlobalPlanner* global_planner, std::vector<Cell>& path, const NodePtr& s,
+                                 const GoalCell& t, int max_iterations = 2000) {
   NullVisitor visitor;
   return findSmoothPath(global_planner, path, s, t, max_iterations, visitor);
 }
 
 // A* to find a path from start to t, true iff it found a path
 template <typename GlobalPlanner, typename Visitor>
-SearchInfo findSmoothPath(GlobalPlanner* global_planner, std::vector<Cell>& path, const NodePtr& s, const GoalCell& t,
-                          int max_iterations, Visitor& visitor) {
+inline SearchInfo findSmoothPath(GlobalPlanner* global_planner, std::vector<Cell>& path, const NodePtr& s,
+                                 const GoalCell& t, int max_iterations, Visitor& visitor) {
   // Initialize containers
   NodePtr best_goal_node;
   visitor.init();

global_planner/src/nodes/global_planner_node.cpp

@@ -403,16 +403,4 @@ void GlobalPlannerNode::publishSetpoint() {
 
 bool GlobalPlannerNode::isCloseToGoal() { return distance(current_goal_, last_pos_) < 1.5; }
 
-}  // namespace global_planner
-
-int main(int argc, char** argv) {
-  ros::init(argc, argv, "global_planner_node");
-
-  ros::NodeHandle nh("~");
-  ros::NodeHandle nh_private("");
-
-  global_planner::GlobalPlannerNode global_planner_node(nh, nh_private);
-
-  ros::spin();
-  return 0;
-}
+}  // namespace global_planner

global_planner/src/nodes/global_planner_node_main.cpp

@@ -0,0 +1,13 @@
+#include "global_planner/global_planner_node.h"
+
+int main(int argc, char** argv) {
+  ros::init(argc, argv, "global_planner_node");
+
+  ros::NodeHandle nh("~");
+  ros::NodeHandle nh_private("");
+
+  global_planner::GlobalPlannerNode global_planner_node(nh, nh_private);
+
+  ros::spin();
+  return 0;
+}