Unify name of state space boundaries to StateSpaceBoundaries

se2_planning/include/se2_planning/OmplPlanner.hpp

@@ -33,7 +33,7 @@ class OmplPlanner : public Planner {
   void getInterpolatedPath(Path* interpolatedPath, unsigned int numPoints) const;
   ompl::geometric::SimpleSetupPtr getSimpleSetup() const;
   void setOmplPlanner(ompl::base::PlannerPtr planner);
-  virtual void updateStateSpaceBounds(const ompl::base::RealVectorBounds& bounds);
+  virtual void updateStateSpaceBoundaries(const ompl::base::RealVectorBounds& bounds);
 
  protected:
   virtual void initializeStateSpace() = 0;

se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp

@@ -59,8 +59,8 @@ class OmplReedsSheppPlanner final : public OmplPlanner {
   void lockStateValidator();
   void unlockStateValidator();
   bool isLocked() const;
-  void updateStateSpaceBounds(const ompl::base::RealVectorBounds& bounds) override;
-  bool satisfiesStateSpaceBounds(const se2_planning::ReedsSheppState& state) const;
+  void updateStateSpaceBoundaries(const ompl::base::RealVectorBounds& bounds) override;
+  bool satisfiesStateSpaceBoundaries(const se2_planning::ReedsSheppState& state) const;
   const ompl::base::RealVectorBounds& getStateSpaceBoundaries() const;
 
  private:

se2_planning/src/OmplPlanner.cpp

@@ -70,7 +70,7 @@ bool OmplPlanner::initialize() {
   return true;
 }
 
-void OmplPlanner::updateStateSpaceBounds(const ompl::base::RealVectorBounds& bounds) {
+void OmplPlanner::updateStateSpaceBoundaries(const ompl::base::RealVectorBounds& bounds) {
   throw std::runtime_error("Not implemented");
 }
 

se2_planning/src/OmplReedsSheppPlanner.cpp

@@ -76,7 +76,7 @@ const ompl::base::RealVectorBounds& OmplReedsSheppPlanner::getStateSpaceBoundari
   return stateSpace_->as<ompl::base::SE2StateSpace>()->getBounds();
 }
 
-void OmplReedsSheppPlanner::updateStateSpaceBounds(const ompl::base::RealVectorBounds& bounds) {
+void OmplReedsSheppPlanner::updateStateSpaceBoundaries(const ompl::base::RealVectorBounds& bounds) {
   bounds_->low[0] = bounds.low[0];
   bounds_->low[1] = bounds.low[1];
   bounds_->high[0] = bounds.high[0];
@@ -90,7 +90,7 @@ void OmplReedsSheppPlanner::updateStateSpaceBounds(const ompl::base::RealVectorB
   //            << bounds_->low[1] << ", " << bounds_->high[1] << "]" << std::endl;
 }
 
-bool OmplReedsSheppPlanner::satisfiesStateSpaceBounds(const se2_planning::ReedsSheppState& state) const {
+bool OmplReedsSheppPlanner::satisfiesStateSpaceBoundaries(const se2_planning::ReedsSheppState& state) const {
   ompl::base::ScopedStatePtr stateOmpl(std::make_shared<ompl::base::ScopedState<> >(stateSpace_));
   auto s = ((*stateOmpl)())->as<ompl::base::SE2StateSpace::StateType>();
   auto rsState = state.as<ReedsSheppState>();

se2_planning_ros/include/se2_planning_ros/OmplReedsSheppPlannerRos.hpp

@@ -43,16 +43,17 @@ class OmplReedsSheppPlannerRos : public PlannerRos {
   void initRos();
   void publishPathNavMsgs() const;
   void publishStartGoalMsgs(const ReedsSheppState& start, const ReedsSheppState& goal) const;
-  void initializeStateSpaceMarker();
-  void publishStateSpaceMarker();
+  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_;
-  visualization_msgs::Marker stateSpaceMarker_;
+
+  visualization_msgs::Marker stateSpaceBoundaryMarker_;
   OmplReedsSheppPlannerRosParameters parameters_;
   ros::ServiceServer planningService_;
   int planSeqNumber_ = -1;

se2_planning_ros/src/OmplReedsSheppPlannerRos.cpp

@@ -23,7 +23,7 @@ void OmplReedsSheppPlannerRos::setParameters(const OmplReedsSheppPlannerRosParam
 bool OmplReedsSheppPlannerRos::initialize() {
   bool result = BASE::initialize();
   initRos();
-  initializeStateSpaceMarker();
+  initializeStateSpaceBoundaryMarker();
   return result;
 }
 bool OmplReedsSheppPlannerRos::plan() {
@@ -65,30 +65,30 @@ bool OmplReedsSheppPlannerRos::planningService(PlanningService::Request& req, Pl
   // TODO expose stateSpaceBoundsMargin_ as param => depends on footprint size?
   // TODO move to OMPLReedsSheppPlanner.cpp?
   const double stateSpaceBoundsMargin_ = parameters_.stateSpaceBoundsMargin_;
-  if (!planner_->as<OmplReedsSheppPlanner>()->satisfiesStateSpaceBounds(start)) {
+  if (!planner_->as<OmplReedsSheppPlanner>()->satisfiesStateSpaceBoundaries(start)) {
     ROS_DEBUG("Initial state not in grid map. Enlarge state space boundaries.");
     const auto bounds = planner_->as<OmplReedsSheppPlanner>()->getStateSpaceBoundaries();
     ompl::base::RealVectorBounds newBounds(reedsSheppStateSpaceDim_);
     newBounds.high[0] = std::max(bounds.high[0], start.x_ + stateSpaceBoundsMargin_);
     newBounds.high[1] = std::max(bounds.high[1], start.y_ + stateSpaceBoundsMargin_);
     newBounds.low[0] = std::min(bounds.low[0], start.x_ - stateSpaceBoundsMargin_);
     newBounds.low[1] = std::min(bounds.low[1], start.y_ - stateSpaceBoundsMargin_);
-    planner_->as<OmplReedsSheppPlanner>()->updateStateSpaceBounds(newBounds);
+    planner_->as<OmplReedsSheppPlanner>()->updateStateSpaceBoundaries(newBounds);
   }
-  if (!planner_->as<OmplReedsSheppPlanner>()->satisfiesStateSpaceBounds(goal)) {
+  if (!planner_->as<OmplReedsSheppPlanner>()->satisfiesStateSpaceBoundaries(goal)) {
     ROS_DEBUG("Goal state not in grid map. Enlarge state space boundaries.");
     const auto bounds = planner_->as<OmplReedsSheppPlanner>()->getStateSpaceBoundaries();
     ompl::base::RealVectorBounds newBounds(reedsSheppStateSpaceDim_);
     newBounds.high[0] = std::max(bounds.high[0], goal.x_ + stateSpaceBoundsMargin_);
     newBounds.high[1] = std::max(bounds.high[1], goal.y_ + stateSpaceBoundsMargin_);
     newBounds.low[0] = std::min(bounds.low[0], goal.x_ - stateSpaceBoundsMargin_);
     newBounds.low[1] = std::min(bounds.low[1], goal.y_ - stateSpaceBoundsMargin_);
-    planner_->as<OmplReedsSheppPlanner>()->updateStateSpaceBounds(newBounds);
+    planner_->as<OmplReedsSheppPlanner>()->updateStateSpaceBoundaries(newBounds);
   }
 
   // TODO move to detach? Better to publish this info for debugging before checking validity of states.
   publishStartGoalMsgs(start, goal);
-  publishStateSpaceMarker();
+  publishStateSpaceBoundaryMarker();
 
   //  Use state validator only after lock mutex is active and state space is updated
   //  Checks only for non-traversable terrain not for state space bounds
@@ -157,47 +157,46 @@ void OmplReedsSheppPlannerRos::publishStartGoalMsgs(const ReedsSheppState& start
   goalPublisher_.publish(goalPose);
 }
 
-void OmplReedsSheppPlannerRos::initializeStateSpaceMarker() {
-  // TODO expose as params?
-  double lineWidth_ = 0.01;
-  std_msgs::ColorRGBA color_;
+void OmplReedsSheppPlannerRos::initializeStateSpaceBoundaryMarker() {
+  double lineWidth = 0.01;
+  std_msgs::ColorRGBA color;
   // No transparency
-  color_.a = 1;
+  color.a = 1;
   // Black
-  color_.r = 0;
-  color_.g = 0;
-  color_.b = 0;
+  color.r = 0;
+  color.g = 0;
+  color.b = 0;
   // Init marker
-  int nVertices_ = 5;
-  stateSpaceMarker_.ns = "state_space";
-  stateSpaceMarker_.lifetime = ros::Duration();
-  stateSpaceMarker_.action = visualization_msgs::Marker::ADD;
-  stateSpaceMarker_.type = visualization_msgs::Marker::LINE_STRIP;
-  stateSpaceMarker_.scale.x = lineWidth_;
-  stateSpaceMarker_.points.resize(nVertices_);  // Initialized to [0.0, 0.0, 0.0]
-  stateSpaceMarker_.colors.resize(nVertices_, color_);
+  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::publishStateSpaceMarker() {
+void OmplReedsSheppPlannerRos::publishStateSpaceBoundaryMarker() {
   // Set marker info.
-  stateSpaceMarker_.header.frame_id = parameters_.pathFrame_;
-  stateSpaceMarker_.header.stamp = planTimeStamp_;
+  stateSpaceBoundaryMarker_.header.frame_id = parameters_.pathFrame_;
+  stateSpaceBoundaryMarker_.header.stamp = planTimeStamp_;
 
   // Set positions of markers.
   const auto bounds = planner_->as<OmplReedsSheppPlanner>()->getStateSpaceBoundaries();
-  stateSpaceMarker_.points[0].x = bounds.low[0];
-  stateSpaceMarker_.points[0].y = bounds.low[1];
-  stateSpaceMarker_.points[1].x = bounds.high[0];
-  stateSpaceMarker_.points[1].y = bounds.low[1];
-  stateSpaceMarker_.points[2].x = bounds.high[0];
-  stateSpaceMarker_.points[2].y = bounds.high[1];
-  stateSpaceMarker_.points[3].x = bounds.low[0];
-  stateSpaceMarker_.points[3].y = bounds.high[1];
+  stateSpaceBoundaryMarker_.points[0].x = bounds.low[0];
+  stateSpaceBoundaryMarker_.points[0].y = bounds.low[1];
+  stateSpaceBoundaryMarker_.points[1].x = bounds.high[0];
+  stateSpaceBoundaryMarker_.points[1].y = bounds.low[1];
+  stateSpaceBoundaryMarker_.points[2].x = bounds.high[0];
+  stateSpaceBoundaryMarker_.points[2].y = bounds.high[1];
+  stateSpaceBoundaryMarker_.points[3].x = bounds.low[0];
+  stateSpaceBoundaryMarker_.points[3].y = bounds.high[1];
   // Close the rectangle with the fifth point
-  stateSpaceMarker_.points[4].x = stateSpaceMarker_.points[0].x;
-  stateSpaceMarker_.points[4].y = stateSpaceMarker_.points[0].y;
+  stateSpaceBoundaryMarker_.points[4].x = stateSpaceBoundaryMarker_.points[0].x;
+  stateSpaceBoundaryMarker_.points[4].y = stateSpaceBoundaryMarker_.points[0].y;
 
-  stateSpacePublisher_.publish(stateSpaceMarker_);
+  stateSpacePublisher_.publish(stateSpaceBoundaryMarker_);
 }
 
 } /* namespace se2_planning */

se2_planning_ros/src/se2_planner_node.cpp

@@ -34,7 +34,7 @@ void gridMapCallback(const grid_map_msgs::GridMap& msg) {
   bounds.high[1] = mapPosition.y() + mapLength.y() / 2.0;
 
   if (!planner->isLocked()) {
-    planner->updateStateSpaceBounds(bounds);
+    planner->updateStateSpaceBoundaries(bounds);
     ROS_DEBUG_STREAM("OMPL State Space Update: pos: " << mapPosition.x() << ", " << mapPosition.y() << ", length: " << mapLength.x() << ", "
                                                       << mapLength.y());
   } else {