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

added moveToPose

parent 51de7e23
Branches
No related tags found
No related merge requests found
......@@ -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());
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment