From e5bc65e1112e4b44ec4823099a13bc7eb4b9caed Mon Sep 17 00:00:00 2001
From: CS <christoph.schroeter1@mailbox.tu-dresden.de>
Date: Wed, 26 Apr 2023 17:08:07 +0200
Subject: [PATCH] added moveToPose
---
src/moveit_sorting_controller.cpp | 42 +++++++++++++++++++++++++++++++
1 file changed, 42 insertions(+)
diff --git a/src/moveit_sorting_controller.cpp b/src/moveit_sorting_controller.cpp
index dc1bcc5..2165537 100644
--- a/src/moveit_sorting_controller.cpp
+++ b/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());
--
GitLab