improved structure and performance

src/TimedPlanner.cpp

@@ -74,8 +74,7 @@ void TimedPlanner::loadCircularTrajectory(double radius, geometry_msgs::Point or
   }
 }
 
-bool TimedPlanner::loadWaypoints() {
-
+bool TimedPlanner::updateWaypoints() {
   if (nextTrajectory.is_initialized()) {
     currentTrajectory = nextTrajectory.get();
     nextTrajectory.reset();
@@ -110,33 +109,28 @@ std::vector<moveit_msgs::RobotTrajectory> TimedPlanner::split(moveit::planning_i
 
 }
 
-void TimedPlanner::doMotion(const ros::NodeHandle &node_handle, moveit::planning_interface::MoveGroupInterface &group) {
-
-  loadWaypoints();
+void TimedPlanner::doMotion() {
+  updateWaypoints();
 
   do {
     for (auto &waypoint : currentTrajectory.waypoints) {
-
       moveit::planning_interface::MoveGroupInterface::Plan plan;
-
-      if (TrajectoryUtil::computePathToPose(group, plan, waypoint.pose, waypoint.mode, 0.8, 0.8)) {
-
-        if (nextTrajectory.is_initialized()) {
-          return;
-        }
-
-        for (auto trajectory : split(group, plan)) {
+      if (TrajectoryUtil::computePathToPose(*group, plan, waypoint.pose, waypoint.mode, 0.8, 0.8)) {
+        for (auto trajectory : split(*group, plan)) {
+          if (nextTrajectory.is_initialized()) {
+            ROS_WARN_STREAM("terminating current trajectory");
+            return;
+          }
           ROS_INFO_STREAM(
               "Moving to the next waypoint with speed factor " << motionSpeedFactor << " in mode " << waypoint.mode);
           TrajectoryUtil::applyMotionSpeedFactor(trajectory, motionSpeedFactor);
-          moveit_msgs::MoveItErrorCodes errorCode = group.execute(trajectory);
+          moveit_msgs::MoveItErrorCodes errorCode = group->execute(trajectory);
           if (errorCode.val != errorCode.SUCCESS) {
             ROS_ERROR_STREAM("Error Code from executing segment: " << errorCode);
           }
         }
       } else {
-        ROS_ERROR_STREAM(
-            "Unable to compute " << waypoint.mode << " path to the next waypoint." << std::endl << waypoint);
+        ROS_ERROR_STREAM("Unable to find " << waypoint.mode << " path to the next waypoint." << std::endl << waypoint);
       }
     }
   } while (currentTrajectory.loop);
@@ -162,11 +156,10 @@ void TimedPlanner::newMotionSpeedFactorCallback(const std_msgs::Float64ConstPtr
 
 }
 
-TimedPlanner::TimedPlanner(moveit::planning_interface::MoveGroupInterface &group, const double defaultVelocity,
-                           std::string defaultPlanningMode) : default_velocity(
-    defaultVelocity), default_planning_mode(std::move(defaultPlanningMode)) {
-
-  ros::NodeHandle node_handle("panda_mqtt_connector");
+TimedPlanner::TimedPlanner(ros::NodeHandle &node_handle, moveit::planning_interface::MoveGroupInterface &group,
+                           const double defaultVelocity, std::string defaultPlanningMode) :
+    node_handle(&node_handle), group(&group), default_velocity(defaultVelocity),
+    default_planning_mode(std::move(defaultPlanningMode)), motionSpeedFactor(defaultVelocity) {
 
   std::string defaultTrajectory = "<no value provided>";
   node_handle.getParam("default_trajectory", defaultTrajectory);
@@ -218,7 +211,7 @@ int main(int argc, char **argv) {
   // setup this ros-node
   ros::init(argc, argv, "timed_planner");
   ros::NodeHandle node_handle("panda_mqtt_connector");
-  ros::AsyncSpinner spinner(1);
+  ros::AsyncSpinner spinner(2);
   spinner.start();
 
   // wait for robot init of robot_state_initializer
@@ -229,15 +222,6 @@ int main(int argc, char **argv) {
   moveit::planning_interface::MoveGroupInterface group("panda_arm");
   moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
 
-  TimedPlanner planner(group, .5, TrajectoryUtil::FLUID_PATH);
-
-  ros::Subscriber sub = node_handle.subscribe("trajectory_update", 1000, &TimedPlanner::newTrajectoryCallback,
-                                              &planner);
-
-  ros::Subscriber velocitySubscriber = node_handle.subscribe("max_velocity", 1000,
-                                                             &TimedPlanner::newMotionSpeedFactorCallback,
-                                                             &planner);
-
   // init motionSpeedFactor
   double factor = -1;
   if (node_handle.getParam("robot_speed_factor", factor)) {
@@ -254,11 +238,18 @@ int main(int argc, char **argv) {
   constructPlate(collision_objects, 5.0, 5.0);
   planning_scene_interface.applyCollisionObjects(collision_objects);
 
+  // everything is set up, we can start the planner now
+  TimedPlanner planner(node_handle, group, .5, TrajectoryUtil::FLUID_PATH);
+  ros::Subscriber sub = node_handle.subscribe("trajectory_update", 1000, &TimedPlanner::newTrajectoryCallback,
+                                              &planner);
+  ros::Subscriber velocitySubscriber = node_handle.subscribe("max_velocity", 1000,
+                                                             &TimedPlanner::newMotionSpeedFactorCallback,
+                                                             &planner);
   while (ros::ok()) {
 
     // execute a trajectory
-    planner.doMotion(node_handle, group);
-
+    planner.doMotion();
+    ros::Rate(100).sleep();
     ros::spinOnce();
   }
 

src/TimedPlanner.h

@@ -23,9 +23,9 @@ public:
   static std::vector<moveit_msgs::RobotTrajectory>
   split(MoveGroupInterface &group, const MoveGroupInterface::Plan &plan);
 
-  void doMotion(const ros::NodeHandle &node_handle, MoveGroupInterface &group);
+  void doMotion();
 
-  TimedPlanner(MoveGroupInterface &group, double defaultVelocity, std::string defaultPlanningMode);
+  TimedPlanner(ros::NodeHandle &node_handle, MoveGroupInterface &group, double defaultVelocity, std::string defaultPlanningMode);
 
   void newTrajectoryCallback(const panda_mqtt_connector::Trajectory::ConstPtr &msg);
   void newMotionSpeedFactorCallback(const std_msgs::Float64ConstPtr &msg);
@@ -33,10 +33,13 @@ public:
 private:
   double motionSpeedFactor;
 
+  std::shared_ptr<ros::NodeHandle> node_handle;
+  std::shared_ptr<moveit::planning_interface::MoveGroupInterface> group;
+
   panda_mqtt_connector::Trajectory currentTrajectory;
   boost::optional<panda_mqtt_connector::Trajectory> nextTrajectory;
 
-  bool loadWaypoints();
+  bool updateWaypoints();
 
   void loadSquareTrajectory();