added moveToPose

src/moveit_sorting_controller.cpp

@@ -265,6 +265,48 @@ int main(int argc, char** argv)
       }
       controller.drop(command.drop().idrobot(), command.drop().idbin(), false);
     }
+    else if (command.has_movetopose()){
+      ROS_INFO_STREAM("[COMMAND] for robot " << command.movetopose().idrobot() << ": Move to position "
+                                             << "x" << command.movetopose().position().x() << " "
+                                             << "y" << command.movetopose().position().y() << " "
+                                             << "z" << command.movetopose().position().z() << " "
+                                             << " and orientation "
+                                             << "x" << command.movetopose().orientation().x() << " "
+                                             << "y" << command.movetopose().orientation().y() << " "
+                                             << "z" << command.movetopose().orientation().z() << " "
+                                             << "w" << command.movetopose().orientation().w());
+      if (command.movetopose().idrobot() != controller.getRobotName())
+      {
+        ROS_DEBUG_STREAM("[COMMAND] IGNORED! Ignoring move-to-pose command for robot "
+                         << command.movetopose().idrobot() << " (this is " << controller.getRobotName() << ")");
+        return;
+      }
+
+      Object* robot = controller.resolveObject(command.movetopose().idrobot());
+      if (!robot)
+      {
+        ROS_ERROR_STREAM("[COMMAND] FAILED! Inconsistent scene, robot '" << command.movetopose().idrobot()
+                                                                         << "' not found!");
+        return;
+      }
+
+      geometry_msgs::Pose pose;
+      pose.orientation.x = command.movetopose().orientation().x();
+      pose.orientation.y = command.movetopose().orientation().y();
+      pose.orientation.z = command.movetopose().orientation().z();
+      pose.orientation.w = command.movetopose().orientation().w();
+      pose.position.x = command.movetopose().position().x();
+      pose.position.y = command.movetopose().position().y();
+      pose.position.z = command.movetopose().position().z();
+      if (controller.moveToPose(*robot, pose, false))
+      {
+        ROS_INFO_STREAM("[COMMAND] SUCCESS! Move-to-pose complete!");
+      }
+      else
+      {
+        ROS_ERROR_STREAM("[COMMAND] FAILED! Move-to-pose did not complete.");
+      }
+    }
     else
     {
       ROS_WARN_STREAM("[COMMAND] IGNORED! Ignoring command of unknown type " << command.msg_case());