From 596ee8c47a86412a980da965ffca5e16005544b2 Mon Sep 17 00:00:00 2001 From: Johannes Mey <johannes.mey@tu-dresden.de> Date: Thu, 19 May 2022 10:34:58 +0200 Subject: [PATCH] update dummy controller --- src/dummy_sorting_controller.cpp | 86 ++++++++++++++++++++++++++++---- 1 file changed, 75 insertions(+), 11 deletions(-) diff --git a/src/dummy_sorting_controller.cpp b/src/dummy_sorting_controller.cpp index cf71fff..8adba80 100644 --- a/src/dummy_sorting_controller.cpp +++ b/src/dummy_sorting_controller.cpp @@ -33,36 +33,49 @@ int main(int argc, char **argv) ros::NodeHandle n("connector_node_ros_ccf"); // namespace where the connection_address is - auto armName = getPrivateParameter<std::string>("arm", "arm1"); - DummyRobotArmController controller{n, NODE_NAME, armName}; + auto robotName = getPrivateParameter<std::string>("arm", "arm1"); + ROS_INFO_STREAM("This cell controls arm " << robotName); + + DummyRobotArmController 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()); mqtt_connection->listen(getParameter<std::string>(n, "topics/selection", "selection")); - std::string commandTopic = getParameter(n, "topics/command", ros::this_node::getName() + "/command"); + auto commandTopic = getParameter(n, "topics/command", ros::this_node::getName() + "/command"); mqtt_connection->listen(commandTopic); - controller.setCommandTopic(commandTopic); + auto other_cell_topic = getPrivateParameter<std::string>("other_cell", "/ceti_cell_2_placeworld/scene/update"); + mqtt_connection->listen(other_cell_topic); controller.addConnection(std::move(mqtt_connection)); + controller.addCallback(other_cell_topic, [&controller](auto msg) { + ROS_WARN_STREAM("Updating scene from other cell"); + Scene scene; + scene.ParseFromString(msg); + 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.json")); + "/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) - { controller.sendScene(); }); - ros::Timer timer = n.createTimer(ros::Duration(10), [&controller]( - const ros::TimerEvent &event) - { controller.sendScene(); }); // send a scene every ten seconds + { + ROS_INFO_STREAM("Sending scene manually (triggered by a message to the 'send_scene' topic)"); + controller.sendScene(); + }); + //ros::Timer timer = n.createTimer(ros::Duration(10), [&controller]( + // const ros::TimerEvent &event) + //{ controller.sendScene(); }); // send a scene every ten seconds auto pick_place_callback = [&controller](const Command& command) { - PickAndPlace pick_place = command.pickandplace(); + 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()); @@ -89,7 +102,19 @@ int main(int argc, char **argv) ROS_WARN_STREAM( "Selected unsupported place object of type " << place_object->type()); } - Object *robot = nullptr; // TODO + if (command.pickandplace().idrobot() != controller.getRobotName()) { + ROS_WARN_STREAM( + "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!"); + 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() << "'!"); @@ -105,6 +130,45 @@ int main(int argc, char **argv) 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() << ")"); + return; + } + + Object *robot = controller.resolveObject(command.evacuate().idrobot()); + if (!robot) + { + ROS_ERROR_STREAM( + "Inconsistent scene, robot '" << command.evacuate().idrobot() << "' not found!"); + return; + } + + geometry_msgs::Pose pose; + pose.orientation.x = 1; + 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 (!zone) + { + ROS_ERROR_STREAM("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."); + } + controller.configureCollaborationZone(*zone, command.configchange().idrobotnewowner() != controller.getRobotName()); } else { ROS_WARN_STREAM("Ignoring command that is not PickAndPlace but " << command.msg_case()); } -- GitLab