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

improved logging

parent d87e4eeb
No related branches found
No related tags found
No related merge requests found
Show whitespace changes
Inline Side-by-side
src/dummy_sorting_controller.cpp
+ 36
− 35
View file @ e00f6ce0
......@@ -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,32 +71,31 @@ 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 "
ROS_DEBUG_STREAM("[COMMAND] IGNORED! Ignoring command pick-place command for robot "
<< command.pickandplace().idrobot() << " (this is " << controller.getRobotName() << ")");
return;
}
......@@ -116,18 +103,19 @@ int main(int argc, char** argv)
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()
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);
......
src/moveit_sorting_controller.cpp
+ 85
− 65
View file @ e00f6ce0
......@@ -25,7 +25,6 @@ using CetiRosToolbox::getPrivateParameter;
int main(int argc, char** argv)
{
GOOGLE_PROTOBUF_VERIFY_VERSION;
ros::init(argc, argv, NODE_NAME);
......@@ -39,8 +38,8 @@ int main(int argc, char **argv)
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);
......@@ -62,84 +61,93 @@ int main(int argc, char **argv)
std::optional<std::string> currentlyPickedBox{};
ros::Subscriber sub = n.subscribe<std_msgs::Empty>("send_scene", 1000, [&controller](
const std_msgs::EmptyConstPtr &msg)
{
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());
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());
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());
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());
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()) {
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());
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 = [&currentlyPickedBox, &controller]()
{
auto sceneUpdateCallback = [&currentlyPickedBox, &controller]() {
if (currentlyPickedBox.has_value())
{
auto resolved = controller.resolveObject(currentlyPickedBox.value());
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment