diff --git a/src/object_locator.cpp b/src/object_locator.cpp
index b23630fc608f01e3efa2f4d5de648ed6ea80353e..6cabdfea7116b3d2f24119e4b4f9722d2924b6eb 100644
--- a/src/object_locator.cpp
+++ b/src/object_locator.cpp
@@ -6,6 +6,8 @@
\date 15.06.22
*/
+#include <cmath>
+
#include <ros/ros.h>
#include <ros/package.h>
#include <apriltag_ros/AprilTagDetectionArray.h>
@@ -17,7 +19,6 @@
#include "ccf/util/NodeUtil.h"
#include "Scene.pb.h"
-#include "google/protobuf/text_format.h"
#include <google/protobuf/util/json_util.h>
#include <google/protobuf/message.h>
@@ -31,6 +32,8 @@ const std::string CELL_BUNDLE = "CETI_TABLE_ONE";
const double TABLE_HEIGHT = 0.89;
const double GRID_SIZE = 0.05;
const double MAX_ERROR = 0.02;
+const bool RISKY_MODE = true;
+const bool SNAP_TO_GRID = true;
double distanceToGrid(geometry_msgs::Point point, double grid_size)
{
@@ -57,26 +60,23 @@ int main(int argc, char** argv)
ros::NodeHandle n("ccf");
ros::Rate loop_rate(200);
- ros::Rate pause_rate(ros::Duration(2)); // seconds
- pause_rate.sleep();
+ ros::Duration(2).sleep(); // sleep for two seconds
DummyRobotArmController controller(n, "object_locator", "nobot");
auto mqttServer = getPrivateParameter<std::string>("mqtt_server", "localhost:1883");
- std::unique_ptr<MqttConnection> mqtt_connection =
+ std::shared_ptr<MqttConnection> mqtt_connection =
std::make_unique<MqttConnection>(mqttServer, ros::this_node::getName());
- mqtt_connection->listen(getParameter<std::string>(n, "topics/selection", "selection"));
- auto other_cell_scene_listener_topic =
- getPrivateParameter<std::string>("other_cell_listener", "/ceti_cell_1"
- "/scene/update");
- auto other_cell_delta_update_topic =
+ auto observed_scene_listener_topic =
+ getPrivateParameter<std::string>("other_cell_listener", "/ceti_cell_1/scene/update");
+ auto observed_scene_delta_update_topic =
getPrivateParameter<std::string>("other_cell_delta_updates", "object_locator/scene/update");
- mqtt_connection->listen(other_cell_scene_listener_topic);
- controller.addConnection(std::move(mqtt_connection));
+ controller.addConnection(mqtt_connection);
+ mqtt_connection->listen(observed_scene_listener_topic);
// subscribe to a scene
- controller.addCallback(other_cell_scene_listener_topic, [&controller](auto msg) {
+ controller.addCallback(observed_scene_listener_topic, [&controller](auto msg) {
ROS_INFO_STREAM("Updating scene from other cell.");
Scene scene;
scene.ParseFromString(msg);
@@ -90,7 +90,7 @@ int main(int argc, char** argv)
"objects."
"json"));
- ROS_ERROR_STREAM("object library contains " << objectLibrary.objects_size() << " objects");
+ ROS_DEBUG_STREAM("object library contains " << objectLibrary.objects_size() << " objects");
std::map<std::string, geometry_msgs::Pose> located_poses;
@@ -161,11 +161,13 @@ int main(int argc, char** argv)
tf2::Quaternion q{ pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w };
std::cout.precision(3);
- if (controller.resolveObject(name))
+ auto existingObject = controller.resolveObject(name); // we need this also later to check if pose has changed
+ if (!RISKY_MODE && existingObject)
{
ROS_DEBUG_STREAM("skipping pose for " << name << " the object is already in the scene.");
continue;
}
+
if (pose.orientation.w < 0.99)
{
ROS_WARN_STREAM("skipping pose for " << name << " because the object is rotated " << pose.orientation << " "
@@ -179,21 +181,48 @@ int main(int argc, char** argv)
continue;
}
double e = distanceToGrid(pose.position, GRID_SIZE);
- ROS_ERROR_STREAM("object " << name << " has an error of " << e);
+ ROS_DEBUG_STREAM("object " << name << " has an error of " << e);
// sanitize pose
- relevant_poses[name] = pose;
- relevant_poses[name].position.z = TABLE_HEIGHT;
- relevant_poses[name].position = closestGridPoint(relevant_poses[name].position, GRID_SIZE);
- relevant_poses[name].orientation.x = 0;
- relevant_poses[name].orientation.y = 0;
- relevant_poses[name].orientation.z = 0;
- relevant_poses[name].orientation.w = 1;
-
- if (located_poses[name] == relevant_poses[name])
+ if (SNAP_TO_GRID)
+ {
+ relevant_poses[name] = pose;
+ relevant_poses[name].position.z = TABLE_HEIGHT;
+ relevant_poses[name].position = closestGridPoint(relevant_poses[name].position, GRID_SIZE);
+ relevant_poses[name].orientation.x = 0;
+ relevant_poses[name].orientation.y = 0;
+ relevant_poses[name].orientation.z = 0;
+ relevant_poses[name].orientation.w = 1;
+
+ if (RISKY_MODE && existingObject)
+ {
+ double dx = existingObject->pos().x() - relevant_poses[name].position.x;
+ double dy = existingObject->pos().y() - relevant_poses[name].position.y;
+ double distance = std::sqrt(dx * dx + dy * dy);
+ if (distance < GRID_SIZE / 2)
+ {
+ ROS_DEBUG_STREAM("Skipping object " << name << " because its location has not changed.");
+ continue;
+ }
+ else
+ {
+ ROS_DEBUG_STREAM("Sending object " << name << " because its location has changed by " << distance);
+ }
+ }
+ }
+
+ if (RISKY_MODE || located_poses[name] == relevant_poses[name])
{
- ROS_DEBUG_STREAM("found the same pose for " + name + " two times in a row: "
- << relevant_poses[name].position.x << ", " << relevant_poses[name].position.y);
+ if (RISKY_MODE)
+ {
+ ROS_DEBUG_STREAM("found the new pose for " + name + ": " << relevant_poses[name].position.x << ", "
+ << relevant_poses[name].position.y);
+ }
+ else
+ {
+ ROS_DEBUG_STREAM("found the same pose for " + name + " two times in a row: "
+ << relevant_poses[name].position.x << ", " << relevant_poses[name].position.y);
+ }
auto object = DummyRobotArmController::resolveObject(name, objectLibrary);
if (!object)
{
@@ -205,29 +234,22 @@ int main(int argc, char** argv)
newObject->mutable_pos()->set_x(located_poses[name].position.x);
newObject->mutable_pos()->set_y(located_poses[name].position.y);
newObject->mutable_pos()->set_z(located_poses[name].position.z + newObject->size().height() / 2);
- newObject->set_mode(Object_Mode_MODE_ADD);
+ newObject->set_mode(existingObject ? Object_Mode_MODE_MODIFY : Object_Mode_MODE_ADD);
newObject->set_state(Object_State_STATE_STATIONARY);
}
}
if (updateScene.objects_size() > 0)
{
- ROS_ERROR_STREAM("Sending scene with " << updateScene.objects_size() << " newly recognized objects.");
- controller.sendToAll(other_cell_delta_update_topic, updateScene.SerializeAsString());
+ ROS_DEBUG_STREAM("Sending scene with " << updateScene.objects_size() << " newly recognized or updated objects.");
+ controller.sendToAll(observed_scene_delta_update_topic, updateScene.SerializeAsString());
}
located_poses = relevant_poses;
});
- // receive a tag position
- // if no "base tag" is contained, abort
- // if the contained objects are already in the robot scene, abort (we only detect new objects)
- // if the object tag is not in the base scene, abort
- // find the drop-of location in the scene that is closest to the
-
while (ros::ok())
{
ros::spinOnce();
-
loop_rate.sleep();
}