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

new object locator with continuous updates for objects

parent d967675f
No related branches found
No related tags found
No related merge requests found
...@@ -6,6 +6,8 @@ ...@@ -6,6 +6,8 @@
\date 15.06.22 \date 15.06.22
*/ */
#include <cmath>
#include <ros/ros.h> #include <ros/ros.h>
#include <ros/package.h> #include <ros/package.h>
#include <apriltag_ros/AprilTagDetectionArray.h> #include <apriltag_ros/AprilTagDetectionArray.h>
...@@ -17,7 +19,6 @@ ...@@ -17,7 +19,6 @@
#include "ccf/util/NodeUtil.h" #include "ccf/util/NodeUtil.h"
#include "Scene.pb.h" #include "Scene.pb.h"
#include "google/protobuf/text_format.h"
#include <google/protobuf/util/json_util.h> #include <google/protobuf/util/json_util.h>
#include <google/protobuf/message.h> #include <google/protobuf/message.h>
...@@ -31,6 +32,8 @@ const std::string CELL_BUNDLE = "CETI_TABLE_ONE"; ...@@ -31,6 +32,8 @@ const std::string CELL_BUNDLE = "CETI_TABLE_ONE";
const double TABLE_HEIGHT = 0.89; const double TABLE_HEIGHT = 0.89;
const double GRID_SIZE = 0.05; const double GRID_SIZE = 0.05;
const double MAX_ERROR = 0.02; 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) double distanceToGrid(geometry_msgs::Point point, double grid_size)
{ {
...@@ -57,26 +60,23 @@ int main(int argc, char** argv) ...@@ -57,26 +60,23 @@ int main(int argc, char** argv)
ros::NodeHandle n("ccf"); ros::NodeHandle n("ccf");
ros::Rate loop_rate(200); 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"); DummyRobotArmController controller(n, "object_locator", "nobot");
auto mqttServer = getPrivateParameter<std::string>("mqtt_server", "localhost:1883"); 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()); std::make_unique<MqttConnection>(mqttServer, ros::this_node::getName());
mqtt_connection->listen(getParameter<std::string>(n, "topics/selection", "selection")); auto observed_scene_listener_topic =
auto other_cell_scene_listener_topic = getPrivateParameter<std::string>("other_cell_listener", "/ceti_cell_1/scene/update");
getPrivateParameter<std::string>("other_cell_listener", "/ceti_cell_1" auto observed_scene_delta_update_topic =
"/scene/update");
auto other_cell_delta_update_topic =
getPrivateParameter<std::string>("other_cell_delta_updates", "object_locator/scene/update"); getPrivateParameter<std::string>("other_cell_delta_updates", "object_locator/scene/update");
mqtt_connection->listen(other_cell_scene_listener_topic); controller.addConnection(mqtt_connection);
controller.addConnection(std::move(mqtt_connection)); mqtt_connection->listen(observed_scene_listener_topic);
// subscribe to a scene // 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."); ROS_INFO_STREAM("Updating scene from other cell.");
Scene scene; Scene scene;
scene.ParseFromString(msg); scene.ParseFromString(msg);
...@@ -90,7 +90,7 @@ int main(int argc, char** argv) ...@@ -90,7 +90,7 @@ int main(int argc, char** argv)
"objects." "objects."
"json")); "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; std::map<std::string, geometry_msgs::Pose> located_poses;
...@@ -161,11 +161,13 @@ int main(int argc, char** argv) ...@@ -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 }; tf2::Quaternion q{ pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w };
std::cout.precision(3); 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."); ROS_DEBUG_STREAM("skipping pose for " << name << " the object is already in the scene.");
continue; continue;
} }
if (pose.orientation.w < 0.99) if (pose.orientation.w < 0.99)
{ {
ROS_WARN_STREAM("skipping pose for " << name << " because the object is rotated " << pose.orientation << " " ROS_WARN_STREAM("skipping pose for " << name << " because the object is rotated " << pose.orientation << " "
...@@ -179,21 +181,48 @@ int main(int argc, char** argv) ...@@ -179,21 +181,48 @@ int main(int argc, char** argv)
continue; continue;
} }
double e = distanceToGrid(pose.position, GRID_SIZE); 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 // sanitize pose
relevant_poses[name] = pose; if (SNAP_TO_GRID)
relevant_poses[name].position.z = TABLE_HEIGHT; {
relevant_poses[name].position = closestGridPoint(relevant_poses[name].position, GRID_SIZE); relevant_poses[name] = pose;
relevant_poses[name].orientation.x = 0; relevant_poses[name].position.z = TABLE_HEIGHT;
relevant_poses[name].orientation.y = 0; relevant_poses[name].position = closestGridPoint(relevant_poses[name].position, GRID_SIZE);
relevant_poses[name].orientation.z = 0; relevant_poses[name].orientation.x = 0;
relevant_poses[name].orientation.w = 1; relevant_poses[name].orientation.y = 0;
relevant_poses[name].orientation.z = 0;
if (located_poses[name] == relevant_poses[name]) 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: " if (RISKY_MODE)
<< relevant_poses[name].position.x << ", " << relevant_poses[name].position.y); {
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); auto object = DummyRobotArmController::resolveObject(name, objectLibrary);
if (!object) if (!object)
{ {
...@@ -205,29 +234,22 @@ int main(int argc, char** argv) ...@@ -205,29 +234,22 @@ int main(int argc, char** argv)
newObject->mutable_pos()->set_x(located_poses[name].position.x); newObject->mutable_pos()->set_x(located_poses[name].position.x);
newObject->mutable_pos()->set_y(located_poses[name].position.y); 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->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); newObject->set_state(Object_State_STATE_STATIONARY);
} }
} }
if (updateScene.objects_size() > 0) if (updateScene.objects_size() > 0)
{ {
ROS_ERROR_STREAM("Sending scene with " << updateScene.objects_size() << " newly recognized objects."); ROS_DEBUG_STREAM("Sending scene with " << updateScene.objects_size() << " newly recognized or updated objects.");
controller.sendToAll(other_cell_delta_update_topic, updateScene.SerializeAsString()); controller.sendToAll(observed_scene_delta_update_topic, updateScene.SerializeAsString());
} }
located_poses = relevant_poses; 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()) while (ros::ok())
{ {
ros::spinOnce(); ros::spinOnce();
loop_rate.sleep(); loop_rate.sleep();
} }
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment