correct error massages and fix bug

Author: AimanHaidar

moveit_planners/pilz_industrial_motion_planner/src/path_free_generator.cpp

@@ -101,8 +101,9 @@ double PathFreeGenerator::computeBlendRadius(const std::vector<KDL::Frame>& wayp
     }
 
     // The maximum feasible radius for this junction
-    double local_max_radius = std::tan(segment_angle(waypoints_[i - 1], waypoints_[i], waypoints_[i + 1]) / 2.0) *
-                              std::min(dist1 / 2.0, dist2 / 2.0);
+    double local_max_radius =
+        std::tan((M_PI - segment_angle(waypoints_[i - 1], waypoints_[i], waypoints_[i + 1])) / 2.0) *
+        std::min(dist1 / 2.0, dist2 / 2.0);
 
     // track the tightest constraint
     // due to KDL::Path_RoundedComposite don't support changing radius

moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp

@@ -141,8 +141,14 @@ void TrajectoryGeneratorFree::plan(const planning_scene::PlanningSceneConstPtr&
   catch (const KDL::Error_MotionPlanning& e)
   {
     RCLCPP_ERROR(getLogger(), "Motion planning error: %s", e.Description());
+    int code = e.GetType();
     std::ostringstream os;
-    os << "waypoints specified in path constraints have three consicutive colinear points";
+    if (code == 3102 || code == 3103)
+      os << "zero distance between two points";
+    else if (code == 3104)
+      os << "waypoints specified in path constraints have three consicutive colinear points";
+    else if (code == 3105 || code == 3106)
+      os << "rounding circle of a point is bigger than the distance with one of the neighbor points";
     throw ConsicutiveColinearWaypoints(os.str());
   }
   // create velocity profile