set initial speed at the right time, use full robot speed

src/TimedPlanner.cpp

@@ -115,7 +115,7 @@ void TimedPlanner::doMotion() {
   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 (TrajectoryUtil::computePathToPose(*group, plan, waypoint.pose, waypoint.mode, 1, 1)) {
         for (auto trajectory : split(*group, plan)) {
           if (nextTrajectory.is_initialized()) {
             ROS_WARN_STREAM("terminating current trajectory");
@@ -222,17 +222,6 @@ int main(int argc, char **argv) {
   moveit::planning_interface::MoveGroupInterface group("panda_arm");
   moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
 
-  // init motionSpeedFactor
-  double factor = -1;
-  if (node_handle.getParam("robot_speed_factor", factor)) {
-    ros::Publisher pub = node_handle.advertise<std_msgs::Float64>("max_velocity", 1000);
-    std_msgs::Float64 velocity;
-    velocity.data = factor;
-    pub.publish(velocity);
-  } else {
-    ROS_WARN_STREAM("No default robot_speed_factor provided!");
-  }
-
   // add the ground plate
   std::vector<moveit_msgs::CollisionObject> collision_objects;
   constructPlate(collision_objects, 5.0, 5.0);
@@ -245,6 +234,18 @@ int main(int argc, char **argv) {
   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)) {
+    ros::Publisher pub = node_handle.advertise<std_msgs::Float64>("max_velocity", 1000);
+    std_msgs::Float64 velocity;
+    velocity.data = factor;
+    pub.publish(velocity);
+  } else {
+    ROS_WARN_STREAM("No default robot_speed_factor provided!");
+  }
+
   while (ros::ok()) {
 
     // execute a trajectory