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

update dummy controller

parent f2f6e008
Branches
No related tags found
No related merge requests found
...@@ -33,36 +33,49 @@ int main(int argc, char **argv) ...@@ -33,36 +33,49 @@ int main(int argc, char **argv)
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 armName = getPrivateParameter<std::string>("arm", "arm1"); auto robotName = getPrivateParameter<std::string>("arm", "arm1");
DummyRobotArmController controller{n, NODE_NAME, armName}; ROS_INFO_STREAM("This cell controls arm " << robotName);
DummyRobotArmController controller{n, NODE_NAME, robotName};
auto mqttServer = getPrivateParameter<std::string>("mqtt_server", "localhost:1883"); auto mqttServer = getPrivateParameter<std::string>("mqtt_server", "localhost:1883");
std::unique_ptr<MqttConnection> std::unique_ptr<MqttConnection>
mqtt_connection = std::make_unique<MqttConnection>(mqttServer, ros::this_node::getName()); mqtt_connection = std::make_unique<MqttConnection>(mqttServer, ros::this_node::getName());
mqtt_connection->listen(getParameter<std::string>(n, "topics/selection", "selection")); 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); 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.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) 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") + 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{}; std::optional<std::string> currentlyPickedBox{};
ros::Subscriber sub = n.subscribe<std_msgs::Empty>("send_scene", 1000, [&controller]( ros::Subscriber sub = n.subscribe<std_msgs::Empty>("send_scene", 1000, [&controller](
const std_msgs::EmptyConstPtr &msg) const std_msgs::EmptyConstPtr &msg)
{ controller.sendScene(); }); {
ros::Timer timer = n.createTimer(ros::Duration(10), [&controller]( ROS_INFO_STREAM("Sending scene manually (triggered by a message to the 'send_scene' topic)");
const ros::TimerEvent &event) controller.sendScene();
{ controller.sendScene(); }); // send a scene every ten seconds });
//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) auto pick_place_callback = [&controller](const Command& command)
{ {
PickAndPlace pick_place = command.pickandplace(); const PickAndPlace& pick_place = command.pickandplace();
if (command.has_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("Got Command to pick object " << pick_place.idpick() << " and place it into " << pick_place.idplace());
Object *pick_object = controller.resolveObject(pick_place.idpick()); Object *pick_object = controller.resolveObject(pick_place.idpick());
...@@ -89,7 +102,19 @@ int main(int argc, char **argv) ...@@ -89,7 +102,19 @@ int main(int argc, char **argv)
ROS_WARN_STREAM( ROS_WARN_STREAM(
"Selected unsupported place object of type " << place_object->type()); "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 (place_object->type() == Object::BIN) {
if (!controller.pickAndDrop(*robot, *pick_object, *place_object, false)) { if (!controller.pickAndDrop(*robot, *pick_object, *place_object, false)) {
ROS_WARN_STREAM("Unable to remove box '" << pick_place.idpick() << "'!"); ROS_WARN_STREAM("Unable to remove box '" << pick_place.idpick() << "'!");
...@@ -105,6 +130,45 @@ int main(int argc, char **argv) ...@@ -105,6 +130,45 @@ int main(int argc, char **argv)
controller.sendScene(); 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 { } else {
ROS_WARN_STREAM("Ignoring command that is not PickAndPlace but " << command.msg_case()); ROS_WARN_STREAM("Ignoring command that is not PickAndPlace but " << command.msg_case());
} }
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment