Merge pull request #821 from ros-planning/fix739kinetic

PR #739 for Kinetic
ros-planning · Dec 7, 2018 · ef762ee · ef762ee
2 parents 3517855 + 5591697
commit ef762ee
Show file tree

Hide file tree

Showing 4 changed files with 53 additions and 19 deletions.
diff --git a/global_planner/cfg/GlobalPlanner.cfg b/global_planner/cfg/GlobalPlanner.cfg
@@ -5,20 +5,29 @@ from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, i
 
 gen = ParameterGenerator()
 
-gen.add("lethal_cost",  int_t,   0, "Lethal Cost",  253, 1, 255)
-gen.add("neutral_cost", int_t,   0, "Neutral Cost",  50, 1, 255)
+gen.add("lethal_cost", int_t, 0, "Lethal Cost", 253, 1, 255)
+gen.add("neutral_cost", int_t, 0, "Neutral Cost", 50, 1, 255)
 gen.add("cost_factor", double_t, 0, "Factor to multiply each cost from costmap by", 3.0, 0.01, 5.0)
 gen.add("publish_potential", bool_t, 0, "Publish Potential Costmap", True)
 
-orientation_enum = gen.enum([ 
-    gen.const("None",        int_t, 0, "No orientations added except goal orientation"),
-    gen.const("Forward",     int_t, 1, "Orientations point to the next point on the path"),
+orientation_enum = gen.enum([
+    gen.const("None", int_t, 0, "No orientations added except goal orientation"),
+    gen.const("Forward", int_t, 1,
+              "Positive x axis points along path, except for the goal orientation"),
     gen.const("Interpolate", int_t, 2, "Orientations are a linear blend of start and goal pose"),
-    gen.const("ForwardThenInterpolate", 
-                             int_t, 3, "Forward orientation until last straightaway, then a linear blend until the goal pose"),
-                            ], "How to set the orientation of each point")
+    gen.const("ForwardThenInterpolate",
+              int_t, 3, "Forward orientation until last straightaway, then a linear blend until the goal pose"),
+    gen.const("Backward", int_t, 4,
+              "Negative x axis points along the path, except for the goal orientation"),
+    gen.const("Leftward", int_t, 5,
+              "Positive y axis points along the path, except for the goal orientation"),
+    gen.const("Rightward", int_t, 6,
+              "Negative y axis points along the path, except for the goal orientation"),
+], "How to set the orientation of each point")
 
-gen.add("orientation_mode",  int_t, 0, "How to set the orientation of each point", 1, 0, 3,
+gen.add("orientation_mode", int_t, 0, "How to set the orientation of each point", 1, 0, 6,
         edit_method=orientation_enum)
+gen.add("orientation_window_size", int_t, 0, "What window to use to determine the orientation based on the "
+        "position derivative specified by the orientation mode", 1, 1, 255)
 
 exit(gen.generate(PACKAGE, "global_planner", "GlobalPlanner"))
diff --git a/global_planner/include/global_planner/orientation_filter.h b/global_planner/include/global_planner/orientation_filter.h
@@ -40,7 +40,7 @@
 
 namespace global_planner {
 
-enum OrientationMode { NONE, FORWARD, INTERPOLATE, FORWARDTHENINTERPOLATE };
+enum OrientationMode { NONE, FORWARD, INTERPOLATE, FORWARDTHENINTERPOLATE, BACKWARD, LEFTWARD, RIGHTWARD };
 
 class OrientationFilter {
     public:
@@ -50,14 +50,17 @@ class OrientationFilter {
         virtual void processPath(const geometry_msgs::PoseStamped& start,
                                  std::vector<geometry_msgs::PoseStamped>& path);
 
-        void pointToNext(std::vector<geometry_msgs::PoseStamped>& path, int index);
+        void setAngleBasedOnPositionDerivative(std::vector<geometry_msgs::PoseStamped>& path, int index);
         void interpolate(std::vector<geometry_msgs::PoseStamped>& path, 
                          int start_index, int end_index);
 
         void setMode(OrientationMode new_mode){ omode_ = new_mode; }
         void setMode(int new_mode){ setMode((OrientationMode) new_mode); }
+
+        void setWindowSize(size_t window_size){ window_size_ = window_size; }
     protected:
-        OrientationMode omode_;        
+        OrientationMode omode_;
+        int window_size_;
 };
 
 } //end namespace global_planner

diff --git a/global_planner/src/orientation_filter.cpp b/global_planner/src/orientation_filter.cpp
@@ -57,7 +57,25 @@ void OrientationFilter::processPath(const geometry_msgs::PoseStamped& start,
     switch(omode_) {
         case FORWARD:
             for(int i=0;i<n-1;i++){
-                pointToNext(path, i);
+                setAngleBasedOnPositionDerivative(path, i);
+            }
+            break;
+        case BACKWARD:
+            for(int i=0;i<n-1;i++){
+                setAngleBasedOnPositionDerivative(path, i);
+                set_angle(&path[i], angles::normalize_angle(getYaw(path[i]) + M_PI));
+            }
+            break;
+        case LEFTWARD:
+            for(int i=0;i<n-1;i++){
+                setAngleBasedOnPositionDerivative(path, i);
+                set_angle(&path[i], angles::normalize_angle(getYaw(path[i]) - M_PI_2));
+            }
+            break;
+        case RIGHTWARD:
+            for(int i=0;i<n-1;i++){
+                setAngleBasedOnPositionDerivative(path, i);
+                set_angle(&path[i], angles::normalize_angle(getYaw(path[i]) + M_PI_2));
             }
             break;
         case INTERPOLATE:
@@ -66,7 +84,7 @@ void OrientationFilter::processPath(const geometry_msgs::PoseStamped& start,
             break;
         case FORWARDTHENINTERPOLATE:
             for(int i=0;i<n-1;i++){
-                pointToNext(path, i);
+                setAngleBasedOnPositionDerivative(path, i);
             }
 
             int i=n-3;
@@ -86,12 +104,15 @@ void OrientationFilter::processPath(const geometry_msgs::PoseStamped& start,
     }
 }
 
-void OrientationFilter::pointToNext(std::vector<geometry_msgs::PoseStamped>& path, int index)
+void OrientationFilter::setAngleBasedOnPositionDerivative(std::vector<geometry_msgs::PoseStamped>& path, int index)
 {
-  double x0 = path[ index ].pose.position.x, 
-         y0 = path[ index ].pose.position.y,
-         x1 = path[index+1].pose.position.x,
-         y1 = path[index+1].pose.position.y;
+  int index0 = std::max(0, index - window_size_);
+  int index1 = std::min((int)path.size() - 1, index + window_size_);
+
+  double x0 = path[index0].pose.position.x,
+         y0 = path[index0].pose.position.y,
+         x1 = path[index1].pose.position.x,
+         y1 = path[index1].pose.position.y;
 
   double angle = atan2(y1-y0,x1-x0);
   set_angle(&path[index], angle);

diff --git a/global_planner/src/planner_core.cpp b/global_planner/src/planner_core.cpp
@@ -171,6 +171,7 @@ void GlobalPlanner::reconfigureCB(global_planner::GlobalPlannerConfig& config, u
     planner_->setFactor(config.cost_factor);
     publish_potential_ = config.publish_potential;
     orientation_filter_->setMode(config.orientation_mode);
+    orientation_filter_->setWindowSize(config.orientation_window_size);
 }
 
 void GlobalPlanner::clearRobotCell(const tf::Stamped<tf::Pose>& global_pose, unsigned int mx, unsigned int my) {