more stringent condition to apply arc instead of rotating in place

fferri · fferri · commit 7c901bc6be17 · 2015-04-06T07:28:54.000+02:00
diff --git a/src/octomap_path_planner_node.cpp b/src/octomap_path_planner_node.cpp
@@ -485,10 +485,10 @@ void OctomapPathPlanner::generateTwistCommand(const geometry_msgs::PointStamped&
 
     const geometry_msgs::Point& p = local_target.point;
 
-    if(p.x < 0)
+    if(p.x < 0 || fabs(p.y) > p.x)
     {
         // turn in place
-        twist.angular.z = 1.0 * twist_angular_gain_;
+        twist.angular.z = (p.y > 0 ? 1 : -1) * twist_angular_gain_;
     }
     else
     {

Original file line number	Diff line number	Diff line change
`@@ -485,10 +485,10 @@ void OctomapPathPlanner::generateTwistCommand(const geometry_msgs::PointStamped&`
`485`	`485`
`486`	`486`	`const geometry_msgs::Point& p = local_target.point;`
`487`	`487`
`488`		`- if(p.x < 0)`
	`488`	`+ if(p.x < 0 \|\| fabs(p.y) > p.x)`
`489`	`489`	`{`
`490`	`490`	`// turn in place`
`491`		`- twist.angular.z = 1.0 * twist_angular_gain_;`
	`491`	`+ twist.angular.z = (p.y > 0 ? 1 : -1) * twist_angular_gain_;`
`492`	`492`	`}`
`493`	`493`	`else`
`494`	`494`	`{`