Skip to content
Snippets Groups Projects
Commit e5bc65e1 authored by Christoph Schröter's avatar Christoph Schröter
Browse files

added moveToPose

parent 51de7e23
No related branches found
No related tags found
No related merge requests found
...@@ -265,6 +265,48 @@ int main(int argc, char** argv) ...@@ -265,6 +265,48 @@ int main(int argc, char** argv)
} }
controller.drop(command.drop().idrobot(), command.drop().idbin(), false); 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 else
{ {
ROS_WARN_STREAM("[COMMAND] IGNORED! Ignoring command of unknown type " << command.msg_case()); ROS_WARN_STREAM("[COMMAND] IGNORED! Ignoring command of unknown type " << command.msg_case());
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment