straighten interface

config/config.yaml

 panda_mqtt_connector:
   server: "tcp://localhost:1883"
-  robot_speed_factor: .15
+  robot_speed_factor: .95
   robot_planning_mode: "fluid_path" # "fluid_path" or "cartesian_path"
+  default_trajectory: "square" # "square" or "circle", everything else is ignored
   topics:
     robotConfig: "robotconfig"
     dataConfig: "dataconfig"

src/TimedPlanner.cpp

@@ -23,12 +23,14 @@ namespace PlannerState {
 
   const double default_velocity = 1.0;
   const std::string default_planning_mode = TrajectoryUtil::FLUID_PATH;
+  const bool useCircle = true;
 
   bool isLooping = true;
 
 }
 
-void constructPlate(std::vector<moveit_msgs::CollisionObject> &collision_objects, double x_dimension, double y_dimension) {
+void
+constructPlate(std::vector<moveit_msgs::CollisionObject> &collision_objects, double x_dimension, double y_dimension) {
 
   moveit_msgs::CollisionObject plate;
 
@@ -52,7 +54,8 @@ void constructPlate(std::vector<moveit_msgs::CollisionObject> &collision_objects
   collision_objects.push_back(plate);
 }
 
-void initCircularTrajectory(double radius, geometry_msgs::Point origin, int granularity) {
+void loadCircularTrajectory(std::vector<geometry_msgs::Pose> &waypoints, double radius, geometry_msgs::Point origin,
+                            int granularity) {
 
   double step_size_rad = 2 * M_PI / granularity;
 
@@ -71,125 +74,68 @@ void initCircularTrajectory(double radius, geometry_msgs::Point origin, int gran
     nextPose.position.y = origin.y + radius * cos(next_rad);
     nextPose.position.z = origin.z;
 
-    PlannerState::raw_trajectory.push_back(nextPose);
-    std::cout << "added: " << nextPose << std::endl;
-  }
+    waypoints.push_back(nextPose);
   }
-
-void
-initRawTrajectory(ros::NodeHandle node_handle, moveit::planning_interface::MoveGroupInterface *group, bool useDefault,
-                  bool useCircularDefault) {
-
-  geometry_msgs::Pose current_pose = group->getCurrentPose().pose;
-
-  if (useCircularDefault) {
-    geometry_msgs::Point origin;
-    origin.z = .3;
-    initCircularTrajectory(0.6, origin, 20);
-    return;
 }
 
-  if (!PlannerState::traj_util.initWaypoints(PlannerState::raw_trajectory, node_handle, current_pose) && useDefault) {
-    // choose a default trajectory
-    geometry_msgs::Pose target_pose_1 = group->getCurrentPose().pose;
-    geometry_msgs::Pose target_pose_2 = group->getCurrentPose().pose;
-    geometry_msgs::Pose target_pose_3 = group->getCurrentPose().pose;
-    geometry_msgs::Pose target_pose_4 = group->getCurrentPose().pose;
-
-    target_pose_2.position.z = 0.6;//0.583;
-    target_pose_2.position.y = -0.6;//63;
-    target_pose_2.position.x = 0;//-0.007;
-    PlannerState::raw_trajectory.push_back(target_pose_2);
+bool loadWaypoints(std::vector<geometry_msgs::Pose> &waypoints, const ros::NodeHandle &node_handle,
+                   geometry_msgs::Pose &initialPose) {
 
-    target_pose_3.position.z = 0.6;//0.691;
-    target_pose_3.position.y = -0.032;
-    target_pose_3.position.x = -0.607;
-    PlannerState::raw_trajectory.push_back(target_pose_3);
+  // regardless if the loading fails, the trajectory is no longer "new" after calling this method
+  node_handle.setParam("new_trajectory_available", false);
 
-    target_pose_4.position.z = 0.6;
-    target_pose_4.position.y = 0.6;//0.509;
-    target_pose_4.position.x = 0;//0.039;
-    PlannerState::raw_trajectory.push_back(target_pose_4);
+  std::vector<double> x_values;
+  std::vector<double> y_values;
+  std::vector<double> z_values;
 
-    target_pose_1.position.z = group->getCurrentPose().pose.position.z;
-    target_pose_1.position.y = group->getCurrentPose().pose.position.y;
-    target_pose_1.position.x = group->getCurrentPose().pose.position.x;
-    PlannerState::raw_trajectory.push_back(target_pose_1);
-  }
+  // check if no trajectory is configured
+  if (!node_handle.getParam("trajectory_pos_x", x_values) ||
+      !node_handle.getParam("trajectory_pos_y", y_values) ||
+      !node_handle.getParam("trajectory_pos_z", z_values)) {
+    return false;
+  } else {
+    for (int i = 0; i < x_values.size(); i++) {
+      geometry_msgs::Pose pose = initialPose;
+      pose.position.x = x_values.at(i);
+      pose.position.y = y_values.at(i);
+      pose.position.z = z_values.at(i);
+      waypoints.push_back(pose);
     }
 
-bool updateRawTrajectory(ros::NodeHandle node_handle, moveit::planning_interface::MoveGroupInterface *group) {
-
-  geometry_msgs::Pose current_pose = group->getCurrentPose().pose;
-  PlannerState::raw_trajectory.clear();
-  node_handle.setParam("new_trajectory_available", false);
-
-  return PlannerState::traj_util.initWaypoints(PlannerState::raw_trajectory, node_handle, current_pose);
+    return true;
   }
-
-void moveRobotToInitialState(ros::NodeHandle node_handle) {
-
-  ROS_INFO("Moving to initial pose of trajectory.");
-  moveit::planning_interface::MoveGroupInterface group("panda_arm");
-
-  ros::Publisher display_publisher = node_handle.
-      advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
-  group.setStartStateToCurrentState();
-
-  std::vector <geometry_msgs::Pose> trajectory_to_init_pose;
-
-  // start at beginning of circular movement
-  tf2::Quaternion orientation; // the RPY constructor is deprecated
-  orientation.setRPY(0, M_PI, 0);
-
-  geometry_msgs::Pose target_pose_1;
-  target_pose_1.orientation = tf2::toMsg(orientation);
-  target_pose_1.position.x = 0;
-  target_pose_1.position.y = .6;
-  target_pose_1.position.z = .3;
-
-  trajectory_to_init_pose.push_back(target_pose_1);
-  group.setPlanningTime(10.0);
-
-  moveit_msgs::RobotTrajectory trajectory;
-  double fraction = group.computeCartesianPath(trajectory_to_init_pose, 0.01, 0.0, trajectory);
-
-  moveit::planning_interface::MoveGroupInterface::Plan cartesian_plan;
-  cartesian_plan.trajectory_ = trajectory;
-  group.execute(cartesian_plan);
 }
 
 void doMotion(const ros::NodeHandle &node_handle, moveit::planning_interface::MoveGroupInterface &group) {
 
-  for (int i = 0; i < PlannerState::raw_trajectory.size(); i++) {
-
   bool newTrajectoryAvailable = false;
   node_handle.getParam("new_trajectory_available", newTrajectoryAvailable);
-    node_handle.getParam("loop_trajectory", PlannerState::isLooping);
 
   if (newTrajectoryAvailable) {
-      if (!updateRawTrajectory(node_handle, &group)) {
-        break;
+    geometry_msgs::Pose current_pose = group.getCurrentPose().pose;
+    PlannerState::raw_trajectory.clear();
+
+    if (!loadWaypoints(PlannerState::raw_trajectory, node_handle, current_pose)) {
+      ROS_ERROR_STREAM("Unable to load new trajectory!");
     }
-      i = 0;
   }
 
-    // wake up if we have trajectory
-    while (PlannerState::raw_trajectory.empty()) {
-      if (newTrajectoryAvailable) {
-        updateRawTrajectory(node_handle, &group);
-        i = 0;
-        break;
-      }
+  if (PlannerState::raw_trajectory.empty()) {
+    return;
   }
 
-    double velocity = 0.0;
-    std::string planning_mode;
+  for (int i = 0; i < PlannerState::raw_trajectory.size(); i++) {
+
+    node_handle.getParam("new_trajectory_available", newTrajectoryAvailable);
+    node_handle.getParam("loop_trajectory", PlannerState::isLooping);
 
+
+    double velocity;
     if (!node_handle.getParam("robot_speed_factor", velocity)) {
       velocity = PlannerState::default_velocity;
     }
 
+    std::string planning_mode;
     if (!node_handle.getParam("robot_planning_mode", planning_mode)) {
       planning_mode = PlannerState::default_planning_mode;
     }
@@ -197,15 +143,18 @@ void doMotion(const ros::NodeHandle &node_handle, moveit::planning_interface::Mo
     ROS_INFO_STREAM("Planning the next trajectory in " << planning_mode << " with velocity " << velocity);
 
     moveit::planning_interface::MoveGroupInterface::Plan plan;
-    PlannerState::traj_util.computePathToPose(group, plan, PlannerState::raw_trajectory.at(i), planning_mode, 0.8, 0.8, velocity);
+
+    if (PlannerState::traj_util.computePathToPose(group, plan, PlannerState::raw_trajectory.at(i), planning_mode, 0.8,
+                                                  0.8, velocity)) {
       group.execute(plan);
+    } else {
+      ROS_ERROR_STREAM("The planner was unable to compute a path to the pose with the given settings.");
+    }
 
     // make sure the robot moves in an infinite circle
     if (i == (PlannerState::raw_trajectory.size() - 1)) {
       if (PlannerState::isLooping) {
         i = -1;
-      } else {
-        return;
       }
     }
   }
@@ -220,27 +169,67 @@ int main(int argc, char **argv) {
   spinner.start();
 
   // wait for robot init of robot_state_initializer
-  ros::Duration(5.0).sleep();
+  ros::Duration(3.0).sleep();
   ROS_INFO(">>>>>>>>>>>>>>>>> START UP FINISHED <<<<<<<<<<<<<<<< ");
-  node_handle.setParam("tud_planner_ready", true);
 
   // Initialize start state of robot and target trajectory.
   moveit::planning_interface::MoveGroupInterface group("panda_arm");
   moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
 
-  //moveRobotToInitialState(node_handle);
-  initRawTrajectory(node_handle, &group, true, true);
   node_handle.getParam("loop_trajectory", PlannerState::isLooping);
 
+  std::string defaultTrajectory = "<none>";
+  node_handle.getParam("default_trajectory", defaultTrajectory);
+
+
+  geometry_msgs::Pose base_pose = group.getCurrentPose().pose;
+
+  if (loadWaypoints(PlannerState::raw_trajectory, node_handle, base_pose)) {
+    ROS_INFO_STREAM("Ignoring default waypoints because there is a trajectory defined by parameters");
+  } else if (defaultTrajectory == "circle") {
+    ROS_INFO_STREAM("loading default trajectory 'circle'");
+    geometry_msgs::Point origin;
+    origin.z = .3;
+    loadCircularTrajectory(PlannerState::raw_trajectory, 0.6, origin, 20);
+  } else if (defaultTrajectory == "square") {
+    ROS_INFO_STREAM("loading default trajectory 'square'");
+    // choose a default trajectory
+    base_pose.position.x = 0.4;
+    base_pose.position.y = 0.4;
+    base_pose.position.z = 0.3;
+    geometry_msgs::Pose target_pose_1 = base_pose;
+    geometry_msgs::Pose target_pose_2 = base_pose;
+    geometry_msgs::Pose target_pose_3 = base_pose;
+    geometry_msgs::Pose target_pose_4 = base_pose;
+
+    PlannerState::raw_trajectory.push_back(target_pose_1);
+
+    target_pose_2.position.y *= -1;
+    PlannerState::raw_trajectory.push_back(target_pose_2);
+
+    target_pose_3.position.x *= -1;
+    target_pose_3.position.y *= -1;
+    PlannerState::raw_trajectory.push_back(target_pose_3);
+
+    target_pose_4.position.x *= -1;
+    PlannerState::raw_trajectory.push_back(target_pose_4);
+  } else {
+    ROS_WARN_STREAM("not loading a default trajectory! provided trajectory: " + defaultTrajectory);
+  }
+
   // add the ground plate
   std::vector<moveit_msgs::CollisionObject> collision_objects;
   constructPlate(collision_objects, 5.0, 5.0);
   planning_scene_interface.applyCollisionObjects(collision_objects);
 
-  // execute the trajectory which consists of single waypoints to allow distinct planning
+  while (ros::ok()) {
+
+    // execute a trajectory
     doMotion(node_handle, group);
 
-  ros::shutdown();
+    ros::spinOnce();
+  }
+
   return 0;
 }
 

src/util/TrajectoryUtil.cpp

@@ -61,6 +61,7 @@ bool TrajectoryUtil::computePathToPose(moveit::planning_interface::MoveGroupInte
   } else if (pathType == FLUID_PATH) {
     if (group.plan(plan) != moveit::planning_interface::MoveItErrorCode::SUCCESS) {
       ROS_ERROR("Computation of fluid path has failed.");
+      ROS_ERROR_STREAM("Path was motion to:\n" << targetPose);
     }
   } else {
     ROS_ERROR("Invalid type of path provided.");
@@ -68,32 +69,8 @@ bool TrajectoryUtil::computePathToPose(moveit::planning_interface::MoveGroupInte
   }
 
   manipulateVelocity(plan, motionSpeedFactor);
-}
-
-bool TrajectoryUtil::initWaypoints(std::vector<geometry_msgs::Pose> &waypoints, ros::NodeHandle &node_handle,
-                                   geometry_msgs::Pose &initialPose) {
-  std::vector<double> x_values;
-  std::vector<double> y_values;
-  std::vector<double> z_values;
-
-  // check if no trajectory is configured
-  if (!node_handle.getParam("trajectory_pos_x", x_values) ||
-      !node_handle.getParam("trajectory_pos_y", y_values) ||
-      !node_handle.getParam("trajectory_pos_z", z_values)) {
-    return false;
-  } else {
-    for (int i = 0; i < x_values.size(); i++) {
-      geometry_msgs::Pose pose = initialPose;
-      pose.position.x = x_values.at(i);
-      pose.position.y = y_values.at(i);
-      pose.position.z = z_values.at(i);
-      waypoints.push_back(pose);
-    }
-
-    node_handle.setParam("new_trajectory_available", false);
   return true;
 }
-}
 
 void TrajectoryUtil::manipulateVelocity(moveit::planning_interface::MoveGroupInterface::Plan &plan, double velocity) {
   ROS_INFO_STREAM("slowing down plan with " << plan.trajectory_.joint_trajectory.points.size() << " joints");

src/util/TrajectoryUtil.h

@@ -27,12 +27,9 @@ public:
                          const std::string &pathType, double maxVelocityFactor,
                          double maxAccelerationFactor, double motionSpeedFactor);
 
-  bool initWaypoints(std::vector<geometry_msgs::Pose> &waypoints, ros::NodeHandle &node_handle,
-                     geometry_msgs::Pose &initialPose);
-
 private:
 
-  void manipulateVelocity(moveit::planning_interface::MoveGroupInterface::Plan &plan, double velocity);
+  static void manipulateVelocity(moveit::planning_interface::MoveGroupInterface::Plan &plan, double velocity);
 };
 
 #endif //PANDA_SIMULATION_TRAJECTORYUTIL_H
\ No newline at end of file