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

fix merge error and autoformat

parent b8e169a0
No related branches found
No related tags found
No related merge requests found
......@@ -58,8 +58,20 @@ int main(int argc, char** argv)
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"));
controller.loadScene(getPrivateParameter<std::string>("scene", ros::package::getPath("ccf_immersive_sorting") + "/con"
"fig/"
"conf"
"ig_"
"scen"
"e_"
"ceti"
"-tab"
"le-"
"1-"
"empt"
"y."
"jso"
"n"));
// only add the scene observer after the base scene has been loaded
controller.addCallback(scene_observer_topic, [&controller](auto msg) {
......@@ -78,8 +90,10 @@ int main(int argc, char** argv)
auto pick_place_callback = [&controller](const Command& command) {
if (command.has_pickandplace())
{
ROS_INFO_STREAM("[COMMAND] for robot " << pick_place.idrobot() << ": Pick object " << pick_place.idpick() << " and place it at " << pick_place.idplace());
Object* pick_object = controller.resolveObject(pick_place.idpick());
ROS_INFO_STREAM("[COMMAND] for robot " << command.pickandplace().idrobot() << ": Pick object "
<< command.pickandplace().idpick() << " and place it at "
<< command.pickandplace().idplace());
Object* pick_object = controller.resolveObject(command.pickandplace().idpick());
if (!pick_object)
{
ROS_ERROR_STREAM("[COMMAND] FAILED! Selected unknown pick object '" << command.pickandplace().idpick() << "'");
......@@ -88,7 +102,8 @@ int main(int argc, char** argv)
Object* place_object = controller.resolveObject(command.pickandplace().idplace());
if (!place_object)
{
ROS_ERROR_STREAM("[COMMAND] FAILED! Selected unknown place object '" << command.pickandplace().idplace() << "'");
ROS_ERROR_STREAM("[COMMAND] FAILED! Selected unknown place object '" << command.pickandplace().idplace()
<< "'");
return;
}
if (pick_object->type() != Object::BOX)
......@@ -141,7 +156,8 @@ int main(int argc, char** argv)
}
else if (command.has_evacuate())
{
ROS_INFO_STREAM("[COMMAND] for robot " << command.evacuate().idrobot() << ": Evacuate " << command.evacuate().idrobot() << " from zone "
ROS_INFO_STREAM("[COMMAND] for robot " << command.evacuate().idrobot() << ": Evacuate "
<< command.evacuate().idrobot() << " from zone "
<< command.evacuate().idcollaborationzone());
if (command.evacuate().idrobot() != controller.getRobotName())
{
......@@ -194,10 +210,10 @@ int main(int argc, char** argv)
}
else if (command.has_pick())
{
Object* robot = controller.resolveObject(command.pick().idrobot());
Object* object = controller.resolveObject(command.pick().idpick());
ROS_INFO_STREAM("[COMMAND] for robot " << command.pick().idrobot() << ": Pick " << object->id() << " using robot " << robot->id());
ROS_INFO_STREAM("[COMMAND] for robot " << command.pick().idrobot() << ": Pick " << object->id() << " using robot "
<< robot->id());
if (!robot)
{
ROS_ERROR_STREAM("[COMMAND] FAILED! Unable to pick using unknown robot '" << command.pick().idrobot() << "'.");
......@@ -214,25 +230,28 @@ int main(int argc, char** argv)
{
Object* robot = controller.resolveObject(command.place().idrobot());
Object* object = controller.resolveObject(command.place().idplace());
ROS_INFO_STREAM("[COMMAND] for robot " << command.place().idrobot() << ": Place object at " << object->id() << " using robot " << robot->id());
ROS_INFO_STREAM("[COMMAND] for robot " << command.place().idrobot() << ": 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() << "'.");
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() << "'.");
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())
{
Object* robot = controller.resolveObject(command.drop().idrobot());
Object* bin = controller.resolveObject(command.drop().idbin());
ROS_INFO_STREAM("[COMMAND] for robot " << command.drop().idrobot() << ": drop object into bin " << bin->id() << " using robot " << robot->id());
ROS_INFO_STREAM("[COMMAND] for robot " << command.drop().idrobot() << ": drop object into bin " << bin->id()
<< " using robot " << robot->id());
if (!robot)
{
ROS_ERROR_STREAM("[COMMAND] FAILED! Unable to drop using unknown robot '" << command.drop().idrobot() << "'.");
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment