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

add initial version of object locator

parent 199066e3
No related branches found
No related tags found
No related merge requests found
......@@ -139,6 +139,7 @@ add_executable(${PROJECT_NAME}_dummy_pickplace_provider src/dummy_pickplace_prov
add_executable(${PROJECT_NAME}_dummy_sorting_controller src/dummy_sorting_controller.cpp ${PROTO_SRCS} ${PROTO_HDRS})
add_executable(${PROJECT_NAME}_main_controller src/main_controller.cpp ${PROTO_SRCS} ${PROTO_HDRS} src/cell.cpp src/action.cpp src/task.cpp src/cell_controller.cpp include/cell_controller.h)
add_executable(${PROJECT_NAME}_moveit_sorting_controller src/moveit_sorting_controller.cpp ${PROTO_SRCS} ${PROTO_HDRS})
add_executable(${PROJECT_NAME}_object_locator src/object_locator.cpp ${PROTO_SRCS} ${PROTO_HDRS})
## Rename C++ executable without prefix
......@@ -150,6 +151,7 @@ set_target_properties(${PROJECT_NAME}_dummy_pickplace_provider PROPERTIES OUTPUT
set_target_properties(${PROJECT_NAME}_dummy_sorting_controller PROPERTIES OUTPUT_NAME dummy_sorting_controller PREFIX "")
set_target_properties(${PROJECT_NAME}_main_controller PROPERTIES OUTPUT_NAME main_controller PREFIX "")
set_target_properties(${PROJECT_NAME}_moveit_sorting_controller PROPERTIES OUTPUT_NAME moveit_sorting_controller PREFIX "")
set_target_properties(${PROJECT_NAME}_object_locator PROPERTIES OUTPUT_NAME object_locator PREFIX "")
## Add cmake target dependencies of the executable
## same as for the library above
......@@ -158,6 +160,7 @@ add_dependencies(${PROJECT_NAME}_dummy_pickplace_provider ${${PROJECT_NAME}_EXPO
add_dependencies(${PROJECT_NAME}_dummy_sorting_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(${PROJECT_NAME}_main_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(${PROJECT_NAME}_moveit_sorting_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(${PROJECT_NAME}_object_locator ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
target_link_libraries(${PROJECT_NAME}_dummy_sorting_controller
......@@ -176,7 +179,9 @@ target_link_libraries(${PROJECT_NAME}_dummy_selection_provider
target_link_libraries(${PROJECT_NAME}_dummy_pickplace_provider
${catkin_LIBRARIES}
)
target_link_libraries(${PROJECT_NAME}_object_locator
${catkin_LIBRARIES}
)
#############
## Install ##
......
<launch>
<!-- MQTT server for communication with client cells -->
<arg name="mqtt_server" default="tcp://localhost:1883" doc="address of the mqtt server for the connection to the controller"/>
<include file="$(find ccf)/launch/noop-sim_setup.launch"/>
<param name="/connector_node_ros_ccf/topics/command" value="/ceti_cell_placeworld/command"/>
<node pkg="ccf_immersive_sorting" type="moveit_sorting_controller" name="ceti_cell_empty" output="screen">
<param name="arm" type="string" value="arm"/>
<param name="scene_observer" type="string" value="object_locator/scene/update"/>
<param name="mqtt_server" type="yaml" value="$(arg mqtt_server)"/>
<param name="scene" type="string" value="$(find ccf_immersive_sorting)/config/config_scene_ceti-table-1-empty.json"/>
</node>
</launch>
<launch>
<arg name="launch_prefix" default="" /> <!-- set to value="gdbserver localhost:10000" for remote debugging -->
<arg name="node_namespace" default="apriltag_ros_continuous_node" />
<arg name="camera_name" default="/usb_cam" />
<arg name="image_topic" default="image_rect" />
<!-- Set parameters -->
<rosparam command="load" file="$(find ccf_immersive_sorting)/config/settings.yaml" ns="$(arg node_namespace)" />
<rosparam command="load" file="$(find ccf_immersive_sorting)/config/tags.yaml" ns="$(arg node_namespace)" />
<node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="log">
<param name="video_device" value="/dev/video2" />
<param name="image_width" value="2560" />
<param name="image_height" value="1440" />
<param name="autofocus" value="0" />
</node>
<node name="image_proc" pkg="image_proc" type="image_proc" ns="usb_cam"/>
<node pkg="apriltag_ros" type="apriltag_ros_continuous_node" name="$(arg node_namespace)" clear_params="true" output="screen" launch-prefix="$(arg launch_prefix)" >
<!-- Remap topics from those used in code to those on the ROS network -->
<remap from="image_rect" to="$(arg camera_name)/$(arg image_topic)" />
<remap from="camera_info" to="$(arg camera_name)/camera_info" />
<param name="publish_tag_detections_image" type="bool" value="true" /> <!-- default: false -->
</node>
</launch>
/*! \file object_locator.cpp
A ROS node that detects objects
\author Johannes Mey
\date 15.06.22
*/
#include <ros/ros.h>
#include <ros/package.h>
#include <apriltag_ros/AprilTagDetectionArray.h>
#include "std_msgs/String.h"
#include "tf2/LinearMath/Transform.h"
#include "ccf/util/NodeUtil.h"
#include "connector.pb.h"
#include "google/protobuf/text_format.h"
#include <google/protobuf/util/json_util.h>
#include <google/protobuf/message.h>
#include <ccf/controller/DummyRobotArmController.h>
#include <ccf/connection/MqttConnection.h>
using CetiRosToolbox::getParameter;
using CetiRosToolbox::getPrivateParameter;
const std::string CELL_BUNDLE = "CETI_TABLE_ONE";
const double TABLE_HEIGHT = 0.89;
const double GRID_SIZE = 0.05;
geometry_msgs::Point closestGridPoint(geometry_msgs::Point point, double grid_size)
{
geometry_msgs::Point grid_point;
grid_point.x = (std::round(point.x / grid_size)) * grid_size;
grid_point.y = (std::round(point.y / grid_size)) * grid_size;
grid_point.z = point.z;
return grid_point;
}
int main(int argc, char** argv)
{
GOOGLE_PROTOBUF_VERIFY_VERSION;
ros::init(argc, argv, "object_locator");
ros::NodeHandle n("ccf");
ros::Rate loop_rate(200);
ros::Rate pause_rate(ros::Duration(2)); // seconds
pause_rate.sleep();
DummyRobotArmController controller(n, "object_locator", "nobot");
auto mqttServer = getPrivateParameter<std::string>("mqtt_server", "localhost:1883");
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 other_cell_scene_listener_topic = getPrivateParameter<std::string>("other_cell_listener", "/ceti_cell_empty/scene/update");
auto other_cell_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));
// subscribe to a scene
controller.addCallback(other_cell_scene_listener_topic, [&controller](auto msg) {
ROS_INFO_STREAM("Updating scene from other cell.");
Scene scene;
scene.ParseFromString(msg);
controller.initScene(scene, false);
});
controller.initScene({}, false);
Scene objectLibrary = RobotArmController::loadSceneFromFile(getPrivateParameter<std::string>("object_library", ros::package::getPath("ccf_immersive_sorting") +
"/config/objects.json"));
ROS_ERROR_STREAM("object library contains " << objectLibrary.objects_size() << " objects");
std::map<std::string, geometry_msgs::Pose> located_poses;
// read the bundles and extract the base pose
std::map<int, std::string> tagBundleTags;
auto tagBundles = CetiRosToolbox::getParameter<XmlRpc::XmlRpcValue>(n, "/apriltag_ros_continuous_node/tag_bundles");
for (int bundle_index = 0; bundle_index < tagBundles.size(); bundle_index++)
{
auto& bundle = tagBundles[bundle_index];
for (int tag_index = 0; tag_index < bundle["layout"].size(); tag_index++)
{
auto& tag = bundle["layout"][tag_index];
tagBundleTags[tag["id"]] = std::string(bundle["name"]);
}
}
ros::Subscriber sub_tages = n.subscribe<apriltag_ros::AprilTagDetectionArray>(
getPrivateParameter<std::string>("tag_topic", "/tag_detections"), 1000, [&](auto& m) {
tf2::Transform base_pose; // pose of the origin relative to the camera
std::map<std::string, geometry_msgs::Pose> allObjectPoses;
// check if the base frame was detected
bool base_tag_found = false;
for (const auto& detection : m->detections)
{
if (tagBundleTags[detection.id[0]] == CELL_BUNDLE)
{ // a base tag was found if an id of the cell bundle was found
base_tag_found = true;
base_pose.setOrigin({ detection.pose.pose.pose.position.x, detection.pose.pose.pose.position.y,
detection.pose.pose.pose.position.z });
base_pose.setRotation({ detection.pose.pose.pose.orientation.x, detection.pose.pose.pose.orientation.y,
detection.pose.pose.pose.orientation.z, detection.pose.pose.pose.orientation.w });
}
else
{
allObjectPoses[tagBundleTags[detection.id[0]]] = detection.pose.pose.pose;
}
}
if (!base_tag_found)
{
ROS_WARN_STREAM("Skipping tag detection because no base tag was found");
return;
}
// make pose relative to origin instead of camera
for (auto& [name, pose] : allObjectPoses)
{
tf2::Transform object({ pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w },
{ pose.position.x, pose.position.y, pose.position.z });
tf2::Transform result = base_pose.inverseTimes(object);
pose.position.x = result.getOrigin().x();
pose.position.y = result.getOrigin().y();
pose.position.z = result.getOrigin().z();
pose.orientation.x = result.getRotation().x();
pose.orientation.y = result.getRotation().y();
pose.orientation.z = result.getRotation().z();
pose.orientation.w = result.getRotation().w();
}
// create a delta scene to send later (if not empty)
Scene updateScene;
updateScene.set_is_delta(true);
// filter object poses
std::map<std::string, geometry_msgs::Pose> relevant_poses;
for (const auto& [name, pose] : allObjectPoses)
{
tf2::Quaternion q{ pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w };
std::cout.precision(3);
if (controller.resolveObject(name)) {
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 << " "
<< q.getAxis() << " " << q.angleShortestPath({ 0, 0, 0, 1 }));
continue;
}
if (std::abs(pose.position.z - TABLE_HEIGHT) > 0.02)
{
ROS_WARN_STREAM("skipping pose for " << name << " because the object is not on table level "
<< pose.position.z - TABLE_HEIGHT);
continue;
}
// 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]) {
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) {
ROS_WARN_STREAM("skipping pose for " << name << " because the object is not in the object library.");
continue;
}
auto newObject = updateScene.add_objects();
*newObject = *object;
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_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());
}
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();
}
return 0;
}
\ No newline at end of file
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment