Move ROS publishers and services to base class PlannerRos, use Planne…

…r state to publish start and goal state

se2_planning_ros/include/se2_planning_ros/OmplReedsSheppPlannerRos.hpp

@@ -38,23 +38,16 @@ class OmplReedsSheppPlannerRos : public PlannerRos {
   bool plan() override;
   void setParameters(const OmplReedsSheppPlannerRosParameters& parameters);
   void publishPath() const final;
+  void publishPathNavMsgs() const final;
+  void publishStartState() const final;
+  void publishGoalState() const final;
+  void publishStateSpaceBoundaryMarker() final;
 
  private:
   void initRos();
-  void publishPathNavMsgs() const;
-  void publishStartGoalMsgs(const ReedsSheppState& start, const ReedsSheppState& goal) const;
   void initializeStateSpaceBoundaryMarker();
-  void publishStateSpaceBoundaryMarker();
   bool planningService(PlanningService::Request& req, PlanningService::Response& res) override;
 
-  ros::Publisher pathNavMsgsPublisher_;
-  ros::Publisher pathPublisher_;
-  ros::Publisher startPublisher_;
-  ros::Publisher goalPublisher_;
-  ros::Publisher stateSpacePublisher_;
-
-  ros::ServiceServer planningService_;
-
   const int reedsSheppStateSpaceDim_ = 2;
   int planSeqNumber_ = -1;
   ros::Time planTimeStamp_{0.0};

se2_planning_ros/include/se2_planning_ros/PlannerRos.hpp

@@ -26,6 +26,10 @@ class PlannerRos : public Planner {
 
   void setPlanningStrategy(std::shared_ptr<Planner> planner);
   virtual void publishPath() const;
+  virtual void publishPathNavMsgs() const;
+  virtual void publishStartState() const;
+  virtual void publishGoalState() const;
+  virtual void publishStateSpaceBoundaryMarker();
 
  protected:
   using PlanningService = se2_navigation_msgs::RequestPathSrv;
@@ -36,6 +40,13 @@ class PlannerRos : public Planner {
 
   ros::NodeHandlePtr nh_;
   std::shared_ptr<Planner> planner_;
+  ros::ServiceServer planningService_;
+
+  ros::Publisher pathNavMsgsPublisher_;
+  ros::Publisher pathPublisher_;
+  ros::Publisher startPublisher_;
+  ros::Publisher goalPublisher_;
+  ros::Publisher stateSpacePublisher_;
 };
 
 } /* namespace se2_planning*/

se2_planning_ros/src/OmplReedsSheppPlannerRos.cpp

@@ -87,7 +87,8 @@ bool OmplReedsSheppPlannerRos::planningService(PlanningService::Request& req, Pl
   }
 
   // TODO move to detach? Better to publish this info for debugging before checking validity of states.
-  publishStartGoalMsgs(start, goal);
+  publishStartState();
+  publishGoalState();
   publishStateSpaceBoundaryMarker();
 
   //  Use state validator only after lock mutex is active and state space is updated
@@ -114,23 +115,32 @@ bool OmplReedsSheppPlannerRos::planningService(PlanningService::Request& req, Pl
 }
 
 void OmplReedsSheppPlannerRos::initRos() {
-  pathNavMsgsPublisher_ = nh_->advertise<nav_msgs::Path>(parameters_.pathNavMsgTopic_, 1, true);
   planningService_ = nh_->advertiseService(parameters_.planningSerivceName_, &OmplReedsSheppPlannerRos::planningService, this);
+  pathNavMsgsPublisher_ = nh_->advertise<nav_msgs::Path>(parameters_.pathNavMsgTopic_, 1, true);
   pathPublisher_ = nh_->advertise<se2_navigation_msgs::PathMsg>(parameters_.pathMsgTopic_, 1);
   startPublisher_ = nh_->advertise<geometry_msgs::PoseStamped>("start", 1);
   goalPublisher_ = nh_->advertise<geometry_msgs::PoseStamped>("goal", 1);
   stateSpacePublisher_ = nh_->advertise<visualization_msgs::Marker>("state_space", 1);
 }
 
-void OmplReedsSheppPlannerRos::publishPathNavMsgs() const {
-  ReedsSheppPath rsPath;
-  planner_->as<OmplPlanner>()->getInterpolatedPath(&rsPath, parameters_.pathNavMsgResolution_);
-  nav_msgs::Path msg = se2_planning::copyAllPoints(rsPath);
-  msg.header.frame_id = parameters_.pathFrame_;
-  msg.header.stamp = planTimeStamp_;
-  msg.header.seq = planSeqNumber_;
-  pathNavMsgsPublisher_.publish(msg);
-  ROS_INFO_STREAM("Publishing ReedsShepp path nav msg, num states: " << msg.poses.size());
+void OmplReedsSheppPlannerRos::initializeStateSpaceBoundaryMarker() {
+  double lineWidth = 0.01;
+  std_msgs::ColorRGBA color;
+  // No transparency
+  color.a = 1;
+  // Black
+  color.r = 0;
+  color.g = 0;
+  color.b = 0;
+  // Init marker
+  int nVertices = 5;
+  stateSpaceBoundaryMarker_.ns = "state_space";
+  stateSpaceBoundaryMarker_.lifetime = ros::Duration();
+  stateSpaceBoundaryMarker_.action = visualization_msgs::Marker::ADD;
+  stateSpaceBoundaryMarker_.type = visualization_msgs::Marker::LINE_STRIP;
+  stateSpaceBoundaryMarker_.scale.x = lineWidth;
+  stateSpaceBoundaryMarker_.points.resize(nVertices);  // Initialized to [0.0, 0.0, 0.0]
+  stateSpaceBoundaryMarker_.colors.resize(nVertices, color);
 }
 
 void OmplReedsSheppPlannerRos::publishPath() const {
@@ -144,37 +154,35 @@ void OmplReedsSheppPlannerRos::publishPath() const {
   ROS_INFO_STREAM("Publishing ReedsShepp path, num states: " << rsPath.numPoints());
 }
 
-void OmplReedsSheppPlannerRos::publishStartGoalMsgs(const ReedsSheppState& start, const ReedsSheppState& goal) const {
+void OmplReedsSheppPlannerRos::publishPathNavMsgs() const {
+  ReedsSheppPath rsPath;
+  planner_->as<OmplPlanner>()->getInterpolatedPath(&rsPath, parameters_.pathNavMsgResolution_);
+  nav_msgs::Path msg = se2_planning::copyAllPoints(rsPath);
+  msg.header.frame_id = parameters_.pathFrame_;
+  msg.header.stamp = planTimeStamp_;
+  msg.header.seq = planSeqNumber_;
+  pathNavMsgsPublisher_.publish(msg);
+  ROS_INFO_STREAM("Publishing ReedsShepp path nav msg, num states: " << msg.poses.size());
+}
+
+void OmplReedsSheppPlannerRos::publishStartState() const {
   geometry_msgs::PoseStamped startPose;
   startPose.header.frame_id = parameters_.pathFrame_;
   startPose.header.stamp = planTimeStamp_;
-  startPose.pose = se2_planning::convert(start);
+  ReedsSheppState startState{};
+  getStartingState(&startState);
+  startPose.pose = se2_planning::convert(startState);
   startPublisher_.publish(startPose);
-  geometry_msgs::PoseStamped goalPose;
-  goalPose.header.frame_id = parameters_.pathFrame_;
-  goalPose.header.stamp = planTimeStamp_;
-  goalPose.pose = se2_planning::convert(goal);
-  goalPublisher_.publish(goalPose);
 }
 
-void OmplReedsSheppPlannerRos::initializeStateSpaceBoundaryMarker() {
-  double lineWidth = 0.01;
-  std_msgs::ColorRGBA color;
-  // No transparency
-  color.a = 1;
-  // Black
-  color.r = 0;
-  color.g = 0;
-  color.b = 0;
-  // Init marker
-  int nVertices = 5;
-  stateSpaceBoundaryMarker_.ns = "state_space";
-  stateSpaceBoundaryMarker_.lifetime = ros::Duration();
-  stateSpaceBoundaryMarker_.action = visualization_msgs::Marker::ADD;
-  stateSpaceBoundaryMarker_.type = visualization_msgs::Marker::LINE_STRIP;
-  stateSpaceBoundaryMarker_.scale.x = lineWidth;
-  stateSpaceBoundaryMarker_.points.resize(nVertices);  // Initialized to [0.0, 0.0, 0.0]
-  stateSpaceBoundaryMarker_.colors.resize(nVertices, color);
+void OmplReedsSheppPlannerRos::publishGoalState() const {
+  geometry_msgs::PoseStamped startPose;
+  startPose.header.frame_id = parameters_.pathFrame_;
+  startPose.header.stamp = planTimeStamp_;
+  ReedsSheppState goalState{};
+  getStartingState(&goalState);
+  startPose.pose = se2_planning::convert(goalState);
+  startPublisher_.publish(startPose);
 }
 
 void OmplReedsSheppPlannerRos::publishStateSpaceBoundaryMarker() {

se2_planning_ros/src/PlannerRos.cpp

@@ -15,31 +15,54 @@ void PlannerRos::publishPath() const {
   throw std::runtime_error("Publish path not implemented");
 }
 
+void PlannerRos::publishPathNavMsgs() const {
+  throw std::runtime_error("Publish path nav msgs not implemented");
+}
+
+void PlannerRos::publishStartState() const {
+  throw std::runtime_error("Publish start state not implemented");
+}
+
+void PlannerRos::publishGoalState() const {
+  throw std::runtime_error("Publish goal state not implemented");
+}
+
+void PlannerRos::publishStateSpaceBoundaryMarker() {
+  throw std::runtime_error("Publish state space boundary marker not implemented");
+}
+
 void PlannerRos::setPlanningStrategy(std::shared_ptr<Planner> planner) {
   planner_ = planner;
 }
 
 void PlannerRos::setStartingState(const State& startingState) {
   planner_->setStartingState(startingState);
 }
+
 void PlannerRos::setGoalState(const State& goalState) {
   planner_->setGoalState(goalState);
 }
+
 bool PlannerRos::plan() {
   return planner_->plan();
 }
+
 void PlannerRos::getPath(Path* path) const {
   planner_->getPath(path);
 }
+
 bool PlannerRos::reset() {
   return planner_->reset();
 }
+
 bool PlannerRos::initialize() {
   return planner_->initialize();
 }
+
 void PlannerRos::getStartingState(State* startingState) const {
   planner_->getStartingState(startingState);
 }
+
 void PlannerRos::getGoalState(State* goalState) const {
   planner_->getGoalState(goalState);
 }