Skip to content
Snippets Groups Projects
Commit b97165a2 authored by Johannes Mey's avatar Johannes Mey
Browse files

add support for pick/place/drop, fix initialization bug

parent 7c93ad52
No related branches found
No related tags found
No related merge requests found
...@@ -56,6 +56,12 @@ int main(int argc, char** argv) ...@@ -56,6 +56,12 @@ int main(int argc, char** argv)
controller.initScene(scene, false); controller.initScene(scene, false);
}); });
ros::WallDuration(5).sleep(); // wait 5 secs to init scene (to give moveit time to load)
controller.loadScene(getPrivateParameter<std::string>("scene", ros::package::getPath("ccf_immersive_sorting") +
"/config/config_scene_ceti-table-1-empty.json"));
// only add the scene observer after the base scene has been loaded
controller.addCallback(scene_observer_topic, [&controller](auto msg) { controller.addCallback(scene_observer_topic, [&controller](auto msg) {
ROS_INFO_STREAM("Updating scene from observer."); ROS_INFO_STREAM("Updating scene from observer.");
Scene scene; Scene scene;
...@@ -63,13 +69,6 @@ int main(int argc, char** argv) ...@@ -63,13 +69,6 @@ int main(int argc, char** argv)
controller.initScene(scene, true); controller.initScene(scene, true);
}); });
ros::WallDuration(5).sleep(); // wait 5 secs to init scene (to give moveit time to load)
controller.loadScene(getPrivateParameter<std::string>("scene", ros::package::getPath("ccf_immersive_sorting") +
"/config/config_scene_ceti-table-placeworld.json"));
std::optional<std::string> currentlyPickedBox{};
ros::Subscriber sub = ros::Subscriber sub =
n.subscribe<std_msgs::Empty>("send_scene", 1000, [&controller](const std_msgs::EmptyConstPtr& msg) { n.subscribe<std_msgs::Empty>("send_scene", 1000, [&controller](const std_msgs::EmptyConstPtr& msg) {
ROS_INFO_STREAM("Sending scene manually (triggered by a message to the 'send_scene' topic)"); ROS_INFO_STREAM("Sending scene manually (triggered by a message to the 'send_scene' topic)");
...@@ -194,25 +193,65 @@ int main(int argc, char** argv) ...@@ -194,25 +193,65 @@ int main(int argc, char** argv)
} }
controller.configureCollaborationZone(*zone, command.configchange().idrobotnewowner()); controller.configureCollaborationZone(*zone, command.configchange().idrobotnewowner());
} }
else else if (command.has_pick())
{ {
ROS_WARN_STREAM("[COMMAND] IGNORED! Ignoring command of unknown type " << command.msg_case());
Object* robot = controller.resolveObject(command.pick().idrobot());
Object* object = controller.resolveObject(command.pick().idpick());
ROS_INFO_STREAM("[COMMAND] Pick " << object->id() << " using robot " << robot->id());
if (!robot)
{
ROS_ERROR_STREAM("[COMMAND] FAILED! Unable to pick using unknown robot '" << command.pick().idrobot() << "'.");
return;
} }
}; if (!object)
controller.reactToCommandMessage(pick_place_callback); {
ROS_ERROR_STREAM("[COMMAND] FAILED! Unable to pick unknown object '" << command.pick().idpick() << "'.");
return;
}
controller.pick(command.pick().idrobot(), command.pick().idpick(), false);
}
else if (command.has_place())
{
Object* robot = controller.resolveObject(command.place().idrobot());
Object* object = controller.resolveObject(command.place().idplace());
ROS_INFO_STREAM("[COMMAND] Place object at " << object->id() << " using robot " << robot->id());
if (!robot)
{
ROS_ERROR_STREAM("[COMMAND] FAILED! Unable to place using unknown robot '" << command.place().idrobot() << "'.");
return;
}
if (!object)
{
ROS_ERROR_STREAM("[COMMAND] FAILED! Unable to place at unknown location '" << command.place().idplace() << "'.");
return;
}
controller.place(command.place().idrobot(), command.place().idplace(), false);
}
else if (command.has_drop())
{
auto sceneUpdateCallback = [&currentlyPickedBox, &controller]() { Object* robot = controller.resolveObject(command.drop().idrobot());
if (currentlyPickedBox.has_value()) Object* bin = controller.resolveObject(command.drop().idbin());
ROS_INFO_STREAM("[COMMAND] drop object into bin " << bin->id() << " using robot " << robot->id());
if (!robot)
{ {
auto resolved = controller.resolveObject(currentlyPickedBox.value()); ROS_ERROR_STREAM("[COMMAND] FAILED! Unable to drop using unknown robot '" << command.drop().idrobot() << "'.");
if (!resolved) return;
}
if (!bin)
{ {
ROS_INFO_STREAM("box " << currentlyPickedBox.value() << " has been removed from the scene!"); ROS_ERROR_STREAM("[COMMAND] FAILED! Unable to drop into unknown bin '" << command.drop().idbin() << "'.");
currentlyPickedBox.reset(); return;
} }
controller.drop(command.drop().idrobot(), command.drop().idbin(), false);
}
else
{
ROS_WARN_STREAM("[COMMAND] IGNORED! Ignoring command of unknown type " << command.msg_case());
} }
}; };
controller.reactToSceneUpdateMessage(sceneUpdateCallback); controller.reactToCommandMessage(pick_place_callback);
ros::spin(); ros::spin();
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment