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

improved logging

parent d87e4eeb
Branches
No related tags found
No related merge requests found
...@@ -56,20 +56,8 @@ int main(int argc, char** argv) ...@@ -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) 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" controller.loadScene(getPrivateParameter<std::string>("scene", ros::package::getPath("ccf_immersive_sorting") +
"fig/" "/config/config_scene_ceti-table-placeworld.json"));
"conf"
"ig_"
"scen"
"e_"
"ceti"
"-tab"
"le-"
"plac"
"ewor"
"ld."
"jso"
"n"));
std::optional<std::string> currentlyPickedBox{}; std::optional<std::string> currentlyPickedBox{};
...@@ -83,32 +71,31 @@ int main(int argc, char** argv) ...@@ -83,32 +71,31 @@ int main(int argc, char** argv)
const 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 " ROS_INFO_STREAM("[COMMAND] Pick object " << pick_place.idpick() << " and place it at " << pick_place.idplace());
<< pick_place.idplace());
Object* pick_object = controller.resolveObject(pick_place.idpick()); Object* pick_object = controller.resolveObject(pick_place.idpick());
if (!pick_object) 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; return;
} }
Object* place_object = controller.resolveObject(pick_place.idplace()); Object* place_object = controller.resolveObject(pick_place.idplace());
if (!place_object) 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; return;
} }
if (pick_object->type() != Object::BOX) 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 && if (place_object->type() != Object::BIN && place_object->type() != Object::DROP_OFF_LOCATION &&
place_object->type() != Object::COLLABORATION_ZONE) 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()) if (command.pickandplace().idrobot() != controller.getRobotName())
{ {
ROS_WARN_STREAM("Ignoring command pick-place command for robot " ROS_DEBUG_STREAM("[COMMAND] IGNORED! Ignoring command pick-place command for robot "
<< command.pickandplace().idrobot() << " (this is " << controller.getRobotName() << ")"); << command.pickandplace().idrobot() << " (this is " << controller.getRobotName() << ")");
return; return;
} }
...@@ -116,18 +103,19 @@ int main(int argc, char** argv) ...@@ -116,18 +103,19 @@ int main(int argc, char** argv)
Object* robot = controller.resolveObject(command.pickandplace().idrobot()); Object* robot = controller.resolveObject(command.pickandplace().idrobot());
if (!robot) 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; 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("[COMMAND] FAILED! Unable to remove box '" << pick_place.idpick() << "'!");
} }
else 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(); controller.sendScene();
} }
} }
...@@ -135,28 +123,31 @@ int main(int argc, char** argv) ...@@ -135,28 +123,31 @@ int main(int argc, char** argv)
{ {
if (!controller.pickAndPlace(*robot, *pick_object, *place_object, false)) 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 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(); controller.sendScene();
} }
} }
} }
else if (command.has_evacuate()) else if (command.has_evacuate())
{ {
ROS_INFO_STREAM("[COMMAND] Evacuate " << command.evacuate().idrobot() << " from zone "
<< command.evacuate().idcollaborationzone());
if (command.evacuate().idrobot() != controller.getRobotName()) if (command.evacuate().idrobot() != controller.getRobotName())
{ {
ROS_WARN_STREAM("Ignoring command pick-place command for robot " << command.evacuate().idrobot() << " (this is " ROS_DEBUG_STREAM("[COMMAND] IGNORED! Ignoring evacuate command for robot "
<< controller.getRobotName() << ")"); << command.evacuate().idrobot() << " (this is " << controller.getRobotName() << ")");
return; return;
} }
Object* robot = controller.resolveObject(command.evacuate().idrobot()); Object* robot = controller.resolveObject(command.evacuate().idrobot());
if (!robot) 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; return;
} }
...@@ -165,28 +156,38 @@ int main(int argc, char** argv) ...@@ -165,28 +156,38 @@ int main(int argc, char** argv)
pose.position.x = robot->pos().x() + .4; pose.position.x = robot->pos().x() + .4;
pose.position.y = robot->pos().y(); pose.position.y = robot->pos().y();
pose.position.z = robot->pos().z() + .4; 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()) else if (command.has_configchange())
{ {
Object* zone = controller.resolveObject(command.configchange().idcollaborationzone()); Object* zone = controller.resolveObject(command.configchange().idcollaborationzone());
ROS_INFO_STREAM("[COMMAND] Change config of zone " << zone << " to owner "
<< command.configchange().idrobotnewowner());
if (!zone) 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; return;
} }
Object* robot = controller.resolveObject(command.configchange().idrobotnewowner()); Object* robot = controller.resolveObject(command.configchange().idrobotnewowner());
if (!robot) if (!robot)
{ {
ROS_WARN_STREAM("Collaboration zone active for unknown robot '" << command.evacuate().idrobot() ROS_WARN_STREAM("[COMMAND] INFO: Collaboration zone active for unknown robot '" << command.evacuate().idrobot()
<< "', so blocked in all."); << "', so blocked in all.");
} }
controller.configureCollaborationZone(*zone, command.configchange().idrobotnewowner()); controller.configureCollaborationZone(*zone, command.configchange().idrobotnewowner());
} }
else 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); controller.reactToCommandMessage(pick_place_callback);
......
...@@ -25,7 +25,6 @@ using CetiRosToolbox::getPrivateParameter; ...@@ -25,7 +25,6 @@ using CetiRosToolbox::getPrivateParameter;
int main(int argc, char** argv) int main(int argc, char** argv)
{ {
GOOGLE_PROTOBUF_VERIFY_VERSION; GOOGLE_PROTOBUF_VERIFY_VERSION;
ros::init(argc, argv, NODE_NAME); ros::init(argc, argv, NODE_NAME);
...@@ -39,8 +38,8 @@ int main(int argc, char **argv) ...@@ -39,8 +38,8 @@ int main(int argc, char **argv)
MoveItRobotArmController controller{ n, NODE_NAME, robotName }; MoveItRobotArmController 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 =
mqtt_connection = std::make_unique<MqttConnection>(mqttServer, ros::this_node::getName()); 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"));
auto commandTopic = getParameter<std::string>(n, "topics/command", "/ceti_cell_placeworld/command"); auto commandTopic = getParameter<std::string>(n, "topics/command", "/ceti_cell_placeworld/command");
mqtt_connection->listen(commandTopic); mqtt_connection->listen(commandTopic);
...@@ -62,84 +61,93 @@ int main(int argc, char **argv) ...@@ -62,84 +61,93 @@ int main(int argc, char **argv)
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 =
const std_msgs::EmptyConstPtr &msg) 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)"); ROS_INFO_STREAM("Sending scene manually (triggered by a message to the 'send_scene' topic)");
controller.sendScene(); controller.sendScene();
}); });
auto pick_place_callback = [&controller](const Command& command) auto pick_place_callback = [&controller](const Command& command) {
{
const 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("[COMMAND] Pick object " << pick_place.idpick() << " and place it at " << pick_place.idplace());
Object* pick_object = controller.resolveObject(pick_place.idpick()); Object* pick_object = controller.resolveObject(pick_place.idpick());
if (!pick_object) if (!pick_object)
{ {
ROS_ERROR_STREAM( ROS_ERROR_STREAM("[COMMAND] FAILED! Selected unknown pick object '" << pick_place.idpick() << "'");
"Selected unknown pick object '" << pick_place.idpick() << "'");
return; return;
} }
Object* place_object = controller.resolveObject(pick_place.idplace()); Object* place_object = controller.resolveObject(pick_place.idplace());
if (!place_object) if (!place_object)
{ {
ROS_ERROR_STREAM( ROS_ERROR_STREAM("[COMMAND] FAILED! Selected unknown place object '" << pick_place.idplace() << "'");
"Selected unknown place object '" << pick_place.idplace() << "'");
return; return;
} }
if (pick_object->type() != Object::BOX) if (pick_object->type() != Object::BOX)
{ {
ROS_WARN_STREAM( ROS_WARN_STREAM("[COMMAND] FAILED! Selected unsupported pick object of type " << pick_object->type());
"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( ROS_WARN_STREAM("[COMMAND] FAILED! Selected unsupported place object of type " << place_object->type());
"Selected unsupported place object of type " << place_object->type());
} }
if (command.pickandplace().idrobot() != controller.getRobotName()) { 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; return;
} }
Object* robot = controller.resolveObject(command.pickandplace().idrobot()); Object* robot = controller.resolveObject(command.pickandplace().idrobot());
if (!robot) if (!robot)
{ {
ROS_ERROR_STREAM( ROS_ERROR_STREAM("[COMMAND] FAILED! Inconsistent scene, robot '" << command.evacuate().idrobot()
"Inconsistent scene, robot '" << command.evacuate().idrobot() << "' not found!"); << "' not found!");
return; return;
} }
if (place_object->type() == Object::BIN) { 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() << "'!"); if (!controller.pickAndDrop(*robot, *pick_object, *place_object, false))
} else { {
ROS_INFO_STREAM("Job is done! '" << pick_place.idpick() << "' is no more."); 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(); 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)) { else if (place_object->type() == Object::DROP_OFF_LOCATION || place_object->type() == Object::COLLABORATION_ZONE)
ROS_WARN_STREAM("Unable to move box '" << pick_place.idpick() << "'!"); {
} else { if (!controller.pickAndPlace(*robot, *pick_object, *place_object, false))
ROS_INFO_STREAM("Job is done! '" << pick_place.idpick() << "' is at its new location."); {
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(); controller.sendScene();
} }
} }
} else if (command.has_evacuate()) { }
else if (command.has_evacuate())
{
if (command.evacuate().idrobot() != controller.getRobotName()) { ROS_INFO_STREAM("[COMMAND] Evacuate " << command.evacuate().idrobot() << " from zone "
ROS_WARN_STREAM( << command.evacuate().idcollaborationzone());
"Ignoring command pick-place command for robot " << command.evacuate().idrobot() << " (this is " << controller.getRobotName() << ")"); if (command.evacuate().idrobot() != controller.getRobotName())
{
ROS_DEBUG_STREAM("[COMMAND] IGNORED! Ignoring evacuate command for robot "
<< command.evacuate().idrobot() << " (this is " << controller.getRobotName() << ")");
return; return;
} }
Object* robot = controller.resolveObject(command.evacuate().idrobot()); Object* robot = controller.resolveObject(command.evacuate().idrobot());
if (!robot) if (!robot)
{ {
ROS_ERROR_STREAM( ROS_ERROR_STREAM("[COMMAND] FAILED! Inconsistent scene, robot '" << command.evacuate().idrobot()
"Inconsistent scene, robot '" << command.evacuate().idrobot() << "' not found!"); << "' not found!");
return; return;
} }
...@@ -148,31 +156,43 @@ int main(int argc, char **argv) ...@@ -148,31 +156,43 @@ int main(int argc, char **argv)
pose.position.x = robot->pos().x() + .4; pose.position.x = robot->pos().x() + .4;
pose.position.y = robot->pos().y(); pose.position.y = robot->pos().y();
pose.position.z = robot->pos().z() + .4; pose.position.z = robot->pos().z() + .4;
controller.moveToPose(*robot, pose, false); if (controller.moveToPose(*robot, pose, false))
{
} else if (command.has_configchange()) { 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()); Object* zone = controller.resolveObject(command.configchange().idcollaborationzone());
ROS_INFO_STREAM("[COMMAND] Change config of zone " << zone->id() << " to owner "
<< command.configchange().idrobotnewowner());
if (!zone) 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; return;
} }
Object* robot = controller.resolveObject(command.configchange().idrobotnewowner()); Object* robot = controller.resolveObject(command.configchange().idrobotnewowner());
if (!robot) if (!robot)
{ {
ROS_WARN_STREAM( ROS_WARN_STREAM("[COMMAND] INFO: Collaboration zone active for unknown robot '" << command.evacuate().idrobot()
"Collaboration zone active for unknown robot '" << command.evacuate().idrobot() << "', so blocked in all."); << "', so blocked in all.");
} }
controller.configureCollaborationZone(*zone, command.configchange().idrobotnewowner()); 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); controller.reactToCommandMessage(pick_place_callback);
auto sceneUpdateCallback = [&currentlyPickedBox, &controller]() auto sceneUpdateCallback = [&currentlyPickedBox, &controller]() {
{
if (currentlyPickedBox.has_value()) if (currentlyPickedBox.has_value())
{ {
auto resolved = controller.resolveObject(currentlyPickedBox.value()); auto resolved = controller.resolveObject(currentlyPickedBox.value());
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment