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
Branches
No related tags found
No related merge requests found
......@@ -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,9 +181,11 @@ 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
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);
......@@ -190,10 +194,35 @@ int main(int argc, char** argv)
relevant_poses[name].orientation.z = 0;
relevant_poses[name].orientation.w = 1;
if (located_poses[name] == relevant_poses[name])
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])
{
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();
}
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment