diff --git a/src/moveit_sorting_controller.cpp b/src/moveit_sorting_controller.cpp index 826b4f6b303248ed98bff84251b38b8a45a9a8f4..aed507d1b7a8362812a5f547f04f15f7779ec12d 100644 --- a/src/moveit_sorting_controller.cpp +++ b/src/moveit_sorting_controller.cpp @@ -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,8 +156,9 @@ 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 " - << command.evacuate().idcollaborationzone()); + ROS_INFO_STREAM("[COMMAND] for robot " << command.evacuate().idrobot() << ": Evacuate " + << command.evacuate().idrobot() << " from zone " + << command.evacuate().idcollaborationzone()); if (command.evacuate().idrobot() != controller.getRobotName()) { ROS_DEBUG_STREAM("[COMMAND] IGNORED! Ignoring evacuate command for robot " @@ -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() << "'.");