diff --git a/src/dummy_sorting_controller.cpp b/src/dummy_sorting_controller.cpp index fffae607e1c0d216924219a643ee6509c4f74975..2ad53cc3cbec4a19f519a7de2b739b894fe33e57 100644 --- a/src/dummy_sorting_controller.cpp +++ b/src/dummy_sorting_controller.cpp @@ -56,20 +56,8 @@ 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") + "/con" - "fig/" - "conf" - "ig_" - "scen" - "e_" - "ceti" - "-tab" - "le-" - "plac" - "ewor" - "ld." - "jso" - "n")); + controller.loadScene(getPrivateParameter<std::string>("scene", ros::package::getPath("ccf_immersive_sorting") + + "/config/config_scene_ceti-table-placeworld.json")); std::optional<std::string> currentlyPickedBox{}; @@ -83,51 +71,51 @@ int main(int argc, char** argv) const PickAndPlace& pick_place = command.pickandplace(); if (command.has_pickandplace()) { - ROS_INFO_STREAM("Got Command to pick object " << pick_place.idpick() << " and place it into " - << pick_place.idplace()); + ROS_INFO_STREAM("[COMMAND] Pick object " << pick_place.idpick() << " and place it at " << pick_place.idplace()); Object* pick_object = controller.resolveObject(pick_place.idpick()); if (!pick_object) { - ROS_ERROR_STREAM("Selected unknown pick object '" << pick_place.idpick() << "'"); + ROS_ERROR_STREAM("[COMMAND] FAILED! Selected unknown pick object '" << pick_place.idpick() << "'"); return; } Object* place_object = controller.resolveObject(pick_place.idplace()); if (!place_object) { - ROS_ERROR_STREAM("Selected unknown place object '" << pick_place.idplace() << "'"); + ROS_ERROR_STREAM("[COMMAND] FAILED! Selected unknown place object '" << pick_place.idplace() << "'"); return; } if (pick_object->type() != Object::BOX) { - ROS_WARN_STREAM("Selected unsupported pick object of type " << pick_object->type()); + ROS_WARN_STREAM("[COMMAND] FAILED! Selected unsupported pick object of type " << pick_object->type()); } if (place_object->type() != Object::BIN && place_object->type() != Object::DROP_OFF_LOCATION && place_object->type() != Object::COLLABORATION_ZONE) { - ROS_WARN_STREAM("Selected unsupported place object of type " << place_object->type()); + ROS_WARN_STREAM("[COMMAND] FAILED! Selected unsupported place object of type " << place_object->type()); } if (command.pickandplace().idrobot() != controller.getRobotName()) { - ROS_WARN_STREAM("Ignoring command pick-place command for robot " - << command.pickandplace().idrobot() << " (this is " << controller.getRobotName() << ")"); + ROS_DEBUG_STREAM("[COMMAND] IGNORED! Ignoring command pick-place command for robot " + << command.pickandplace().idrobot() << " (this is " << controller.getRobotName() << ")"); return; } Object* robot = controller.resolveObject(command.pickandplace().idrobot()); if (!robot) { - ROS_ERROR_STREAM("Inconsistent scene, robot '" << command.evacuate().idrobot() << "' not found!"); + ROS_ERROR_STREAM("[COMMAND] FAILED! Inconsistent scene, robot '" << command.evacuate().idrobot() + << "' not found!"); return; } if (place_object->type() == Object::BIN) { if (!controller.pickAndDrop(*robot, *pick_object, *place_object, false)) { - ROS_WARN_STREAM("Unable to remove box '" << pick_place.idpick() << "'!"); + ROS_WARN_STREAM("[COMMAND] FAILED! Unable to remove box '" << pick_place.idpick() << "'!"); } else { - ROS_INFO_STREAM("Job is done! '" << pick_place.idpick() << "' is no more."); + ROS_INFO_STREAM("[COMMAND] SUCCESS! '" << pick_place.idpick() << "' is no more."); controller.sendScene(); } } @@ -135,28 +123,31 @@ int main(int argc, char** argv) { if (!controller.pickAndPlace(*robot, *pick_object, *place_object, false)) { - ROS_WARN_STREAM("Unable to move box '" << pick_place.idpick() << "'!"); + ROS_WARN_STREAM("[COMMAND] FAILED! Unable to move box '" << pick_place.idpick() << "'!"); } else { - ROS_INFO_STREAM("Job is done! '" << pick_place.idpick() << "' is at its new location."); + ROS_INFO_STREAM("[COMMAND] SUCCESS! " << pick_place.idpick() << "' is at its new location."); controller.sendScene(); } } } else if (command.has_evacuate()) { + ROS_INFO_STREAM("[COMMAND] Evacuate " << command.evacuate().idrobot() << " from zone " + << command.evacuate().idcollaborationzone()); if (command.evacuate().idrobot() != controller.getRobotName()) { - ROS_WARN_STREAM("Ignoring command pick-place command for robot " << command.evacuate().idrobot() << " (this is " - << controller.getRobotName() << ")"); + ROS_DEBUG_STREAM("[COMMAND] IGNORED! Ignoring evacuate command for robot " + << command.evacuate().idrobot() << " (this is " << controller.getRobotName() << ")"); return; } Object* robot = controller.resolveObject(command.evacuate().idrobot()); if (!robot) { - ROS_ERROR_STREAM("Inconsistent scene, robot '" << command.evacuate().idrobot() << "' not found!"); + ROS_ERROR_STREAM("[COMMAND] FAILED! Inconsistent scene, robot '" << command.evacuate().idrobot() + << "' not found!"); return; } @@ -165,28 +156,38 @@ int main(int argc, char** argv) pose.position.x = robot->pos().x() + .4; pose.position.y = robot->pos().y(); pose.position.z = robot->pos().z() + .4; - controller.moveToPose(*robot, pose, false); + if (controller.moveToPose(*robot, pose, false)) + { + ROS_INFO_STREAM("[COMMAND] SUCCESS! Evacuation complete!"); + } + else + { + ROS_ERROR_STREAM("[COMMAND] FAILED! Evacuation did not complete."); + } } else if (command.has_configchange()) { Object* zone = controller.resolveObject(command.configchange().idcollaborationzone()); + ROS_INFO_STREAM("[COMMAND] Change config of zone " << zone << " to owner " + << command.configchange().idrobotnewowner()); if (!zone) { - ROS_ERROR_STREAM("Unable to configure unknown collaboration zone '" << command.evacuate().idrobot() << "'."); + ROS_ERROR_STREAM("[COMMAND] FAILED! Unable to configure unknown collaboration zone '" + << command.evacuate().idrobot() << "'."); return; } Object* robot = controller.resolveObject(command.configchange().idrobotnewowner()); if (!robot) { - ROS_WARN_STREAM("Collaboration zone active for unknown robot '" << command.evacuate().idrobot() - << "', so blocked in all."); + ROS_WARN_STREAM("[COMMAND] INFO: Collaboration zone active for unknown robot '" << command.evacuate().idrobot() + << "', so blocked in all."); } controller.configureCollaborationZone(*zone, command.configchange().idrobotnewowner()); } else { - ROS_WARN_STREAM("Ignoring command that is not PickAndPlace but " << command.msg_case()); + ROS_WARN_STREAM("[COMMAND] IGNORED! Ignoring command of unknown type " << command.msg_case()); } }; controller.reactToCommandMessage(pick_place_callback); diff --git a/src/moveit_sorting_controller.cpp b/src/moveit_sorting_controller.cpp index 03f2ca80a1f04d5a3238d878c10cdce1d1ada372..793bed6bb8df47123f095492ca764110630d129c 100644 --- a/src/moveit_sorting_controller.cpp +++ b/src/moveit_sorting_controller.cpp @@ -23,24 +23,23 @@ std::string NODE_NAME = "ceti_cell_placeworld"; using CetiRosToolbox::getParameter; using CetiRosToolbox::getPrivateParameter; -int main(int argc, char **argv) +int main(int argc, char** argv) { - GOOGLE_PROTOBUF_VERIFY_VERSION; ros::init(argc, argv, NODE_NAME); NODE_NAME = ros::this_node::getName(); - ros::NodeHandle n("connector_node_ros_ccf"); // namespace where the connection_address is + ros::NodeHandle n("connector_node_ros_ccf"); // namespace where the connection_address is auto robotName = getPrivateParameter<std::string>("arm", "arm1"); ROS_INFO_STREAM("This cell controls arm " << robotName); - MoveItRobotArmController controller{n, NODE_NAME, robotName}; + MoveItRobotArmController controller{ n, NODE_NAME, robotName }; auto mqttServer = getPrivateParameter<std::string>("mqtt_server", "localhost:1883"); - std::unique_ptr<MqttConnection> - mqtt_connection = std::make_unique<MqttConnection>(mqttServer, ros::this_node::getName()); + std::unique_ptr<MqttConnection> mqtt_connection = + std::make_unique<MqttConnection>(mqttServer, ros::this_node::getName()); mqtt_connection->listen(getParameter<std::string>(n, "topics/selection", "selection")); auto commandTopic = getParameter<std::string>(n, "topics/command", "/ceti_cell_placeworld/command"); mqtt_connection->listen(commandTopic); @@ -55,91 +54,100 @@ int main(int argc, char **argv) controller.initScene(scene, false); }); - ros::WallDuration(5).sleep(); // wait 5 secs to init scene (to give moveit time to load) + 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 = 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)"); - controller.sendScene(); - }); + ros::Subscriber sub = + 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)"); + controller.sendScene(); + }); - auto pick_place_callback = [&controller](const Command& command) - { + auto pick_place_callback = [&controller](const Command& command) { const PickAndPlace& pick_place = command.pickandplace(); - if (command.has_pickandplace()) { - ROS_INFO_STREAM("Got Command to pick object " << pick_place.idpick() << " and place it into " << pick_place.idplace()); - Object *pick_object = controller.resolveObject(pick_place.idpick()); + if (command.has_pickandplace()) + { + ROS_INFO_STREAM("[COMMAND] Pick object " << pick_place.idpick() << " and place it at " << pick_place.idplace()); + Object* pick_object = controller.resolveObject(pick_place.idpick()); if (!pick_object) { - ROS_ERROR_STREAM( - "Selected unknown pick object '" << pick_place.idpick() << "'"); + ROS_ERROR_STREAM("[COMMAND] FAILED! Selected unknown pick object '" << pick_place.idpick() << "'"); return; } - Object *place_object = controller.resolveObject(pick_place.idplace()); + Object* place_object = controller.resolveObject(pick_place.idplace()); if (!place_object) { - ROS_ERROR_STREAM( - "Selected unknown place object '" << pick_place.idplace() << "'"); + ROS_ERROR_STREAM("[COMMAND] FAILED! Selected unknown place object '" << pick_place.idplace() << "'"); return; } if (pick_object->type() != Object::BOX) { - ROS_WARN_STREAM( - "Selected unsupported pick object of type " << pick_object->type()); + ROS_WARN_STREAM("[COMMAND] FAILED! Selected unsupported pick object of type " << pick_object->type()); } - if (place_object->type() != Object::BIN && place_object->type() != Object::DROP_OFF_LOCATION && place_object->type() != Object::COLLABORATION_ZONE) + if (place_object->type() != Object::BIN && place_object->type() != Object::DROP_OFF_LOCATION && + place_object->type() != Object::COLLABORATION_ZONE) { - ROS_WARN_STREAM( - "Selected unsupported place object of type " << place_object->type()); + ROS_WARN_STREAM("[COMMAND] FAILED! Selected unsupported place object of type " << place_object->type()); } - if (command.pickandplace().idrobot() != controller.getRobotName()) { - ROS_WARN_STREAM( - "Ignoring command pick-place command for robot " << command.pickandplace().idrobot() << " (this is " << controller.getRobotName() << ")"); + if (command.pickandplace().idrobot() != controller.getRobotName()) + { + ROS_DEBUG_STREAM("[COMMAND] IGNORED! Ignoring command pick-place command for robot " + << command.pickandplace().idrobot() << " (this is " << controller.getRobotName() << ")"); return; } - Object *robot = controller.resolveObject(command.pickandplace().idrobot()); + Object* robot = controller.resolveObject(command.pickandplace().idrobot()); if (!robot) { - ROS_ERROR_STREAM( - "Inconsistent scene, robot '" << command.evacuate().idrobot() << "' not found!"); + ROS_ERROR_STREAM("[COMMAND] FAILED! Inconsistent scene, robot '" << command.evacuate().idrobot() + << "' not found!"); return; } - if (place_object->type() == Object::BIN) { - if (!controller.pickAndDrop(*robot, *pick_object, *place_object, false)) { - ROS_WARN_STREAM("Unable to remove box '" << pick_place.idpick() << "'!"); - } else { - ROS_INFO_STREAM("Job is done! '" << pick_place.idpick() << "' is no more."); + if (place_object->type() == Object::BIN) + { + if (!controller.pickAndDrop(*robot, *pick_object, *place_object, false)) + { + ROS_WARN_STREAM("[COMMAND] FAILED! Unable to remove box '" << pick_place.idpick() << "'!"); + } + else + { + ROS_INFO_STREAM("[COMMAND] SUCCESS! '" << pick_place.idpick() << "' is no more."); controller.sendScene(); } - } else if (place_object->type() == Object::DROP_OFF_LOCATION || place_object->type() == Object::COLLABORATION_ZONE) { - if (!controller.pickAndPlace(*robot, *pick_object, *place_object, false)) { - ROS_WARN_STREAM("Unable to move box '" << pick_place.idpick() << "'!"); - } else { - ROS_INFO_STREAM("Job is done! '" << pick_place.idpick() << "' is at its new location."); + } + else if (place_object->type() == Object::DROP_OFF_LOCATION || place_object->type() == Object::COLLABORATION_ZONE) + { + if (!controller.pickAndPlace(*robot, *pick_object, *place_object, false)) + { + ROS_WARN_STREAM("[COMMAND] FAILED! Unable to move box '" << pick_place.idpick() << "'!"); + } + else + { + ROS_INFO_STREAM("[COMMAND] SUCCESS! " << pick_place.idpick() << "' is at its new location."); controller.sendScene(); } } - } else if (command.has_evacuate()) { - - - if (command.evacuate().idrobot() != controller.getRobotName()) { - ROS_WARN_STREAM( - "Ignoring command pick-place command for robot " << command.evacuate().idrobot() << " (this is " << controller.getRobotName() << ")"); + } + else if (command.has_evacuate()) + { + ROS_INFO_STREAM("[COMMAND] 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 " + << command.evacuate().idrobot() << " (this is " << controller.getRobotName() << ")"); return; } - Object *robot = controller.resolveObject(command.evacuate().idrobot()); + Object* robot = controller.resolveObject(command.evacuate().idrobot()); if (!robot) { - ROS_ERROR_STREAM( - "Inconsistent scene, robot '" << command.evacuate().idrobot() << "' not found!"); + ROS_ERROR_STREAM("[COMMAND] FAILED! Inconsistent scene, robot '" << command.evacuate().idrobot() + << "' not found!"); return; } @@ -148,31 +156,43 @@ int main(int argc, char **argv) pose.position.x = robot->pos().x() + .4; pose.position.y = robot->pos().y(); pose.position.z = robot->pos().z() + .4; - controller.moveToPose(*robot, pose, false); - - } else if (command.has_configchange()) { - Object *zone = controller.resolveObject(command.configchange().idcollaborationzone()); + if (controller.moveToPose(*robot, pose, false)) + { + ROS_INFO_STREAM("[COMMAND] SUCCESS! Evacuation complete!"); + } + else + { + ROS_ERROR_STREAM("[COMMAND] FAILED! Evacuation did not complete."); + } + } + else if (command.has_configchange()) + { + Object* zone = controller.resolveObject(command.configchange().idcollaborationzone()); + ROS_INFO_STREAM("[COMMAND] Change config of zone " << zone->id() << " to owner " + << command.configchange().idrobotnewowner()); if (!zone) { - ROS_ERROR_STREAM("Unable to configure unknown collaboration zone '" << command.evacuate().idrobot() << "'."); + ROS_ERROR_STREAM("[COMMAND] FAILED! Unable to configure unknown collaboration zone '" + << command.evacuate().idrobot() << "'."); return; } - Object *robot = controller.resolveObject(command.configchange().idrobotnewowner()); + Object* robot = controller.resolveObject(command.configchange().idrobotnewowner()); if (!robot) { - ROS_WARN_STREAM( - "Collaboration zone active for unknown robot '" << command.evacuate().idrobot() << "', so blocked in all."); + ROS_WARN_STREAM("[COMMAND] INFO: Collaboration zone active for unknown robot '" << command.evacuate().idrobot() + << "', so blocked in all."); } controller.configureCollaborationZone(*zone, command.configchange().idrobotnewowner()); - } else { - ROS_WARN_STREAM("Ignoring command that is not PickAndPlace but " << command.msg_case()); + } + else + { + ROS_WARN_STREAM("[COMMAND] IGNORED! Ignoring command of unknown type " << command.msg_case()); } }; controller.reactToCommandMessage(pick_place_callback); - auto sceneUpdateCallback = [¤tlyPickedBox, &controller]() - { + auto sceneUpdateCallback = [¤tlyPickedBox, &controller]() { if (currentlyPickedBox.has_value()) { auto resolved = controller.resolveObject(currentlyPickedBox.value());