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

initial version of table position detection

parent 52b05de7
No related branches found
No related tags found
No related merge requests found
......@@ -141,6 +141,7 @@ add_executable(${PROJECT_NAME}_main_controller src/main_controller.cpp ${PROTO_S
add_executable(${PROJECT_NAME}_virtual_scene_provider src/virtual_scene_provider.cpp ${PROTO_SRCS} ${PROTO_HDRS})
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})
add_executable(${PROJECT_NAME}_table_locator src/table_locator.cpp ${PROTO_SRCS} ${PROTO_HDRS})
## Rename C++ executable without prefix
......@@ -154,6 +155,7 @@ set_target_properties(${PROJECT_NAME}_main_controller PROPERTIES OUTPUT_NAME mai
set_target_properties(${PROJECT_NAME}_virtual_scene_provider PROPERTIES OUTPUT_NAME virtual_scene_provider 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 "")
set_target_properties(${PROJECT_NAME}_table_locator PROPERTIES OUTPUT_NAME table_locator PREFIX "")
## Add cmake target dependencies of the executable
## same as for the library above
......@@ -164,6 +166,7 @@ add_dependencies(${PROJECT_NAME}_main_controller ${${PROJECT_NAME}_EXPORTED_TARG
add_dependencies(${PROJECT_NAME}_virtual_scene_provider ${${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})
add_dependencies(${PROJECT_NAME}_table_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
......@@ -188,6 +191,9 @@ target_link_libraries(${PROJECT_NAME}_dummy_pickplace_provider
target_link_libraries(${PROJECT_NAME}_object_locator
${catkin_LIBRARIES}
)
target_link_libraries(${PROJECT_NAME}_table_locator
${catkin_LIBRARIES}
)
#############
## Install ##
......
File added
......@@ -58,6 +58,16 @@ tag_bundles:
{ id: 3, size: .08, x: 0.3, y: 0.3, z: 0.890, qw: 0.707, qx: 0, qy: 0, qz: 0.707 }
]
},
{
name: 'CETI_TABLE_TWO',
layout:
[
{ id: 100, size: .08, x: -0.3, y: -0.3, z: 0.890, qw: 0.707, qx: 0, qy: 0, qz: 0.707 },
{ id: 101, size: .08, x: -0.3, y: 0.3, z: 0.890, qw: 0.707, qx: 0, qy: 0, qz: 0.707 },
{ id: 102, size: .08, x: 0.3, y: -0.3, z: 0.890, qw: 0.707, qx: 0, qy: 0, qz: 0.707 },
{ id: 103, size: .08, x: 0.3, y: 0.3, z: 0.890, qw: 0.707, qx: 0, qy: 0, qz: 0.707 }
]
},
{ name: 'smallRed65', layout: [ { id: 65, size: .024, x: 0.016, y: 0, z: 0.0350, qw: 0.5, qx: 0.5, qy: 0.5, qz: 0.5 } ] },
{ name: 'smallRed70', layout: [ { id: 70, size: .024, x: 0.016, y: 0, z: 0.0350, qw: 0.5, qx: 0.5, qy: 0.5, qz: 0.5 } ] },
{ name: 'bigRed7', layout: [ { id: 7, size: .052, x: 0.016, y: 0, z: 0.0525, qw: 0.5, qx: 0.5, qy: 0.5, qz: 0.5 } ] },
......
<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"/>
<arg name="device" default="/dev/video0"/>
<!-- 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" respawn="true" respawn_delay="3">
<param name="video_device" value="$(arg device)"/>
<param name="image_width" value="2560"/>
<param name="image_height" value="1440"/>
<param name="autofocus" value="1"/>
<param name="pixel_format" value="mjpeg"/>
<param name="framerate" value="1"/>
<param name="autofocus" value="true"/>
</node>
<node name="image_proc" pkg="image_proc" type="image_proc" ns="usb_cam" respawn="true"/>
<node pkg="apriltag_ros" type="apriltag_ros_continuous_node" name="$(arg node_namespace)" clear_params="true"
output="screen" launch-prefix="$(arg launch_prefix)" respawn="true">
<!-- 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>
<node pkg="ccf_immersive_sorting" type="table_locator" name="table_locator" respawn="true"/>
</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;
int main(int argc, char** argv)
{
GOOGLE_PROTOBUF_VERIFY_VERSION;
ros::init(argc, argv, "table_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());
controller.initScene({}, false);
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;
}
ROS_INFO_STREAM("base tag and " << allObjectPoses.size() << " object poses found!");
// make pose relative to origin instead of camera
for (auto& [name, pose] : allObjectPoses)
{
ROS_INFO_STREAM("make pose " << name << "relative");
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);
double roll, pitch, yaw;
tf2::Matrix3x3 resultMatrix(result.getRotation());
resultMatrix.getRPY(roll, pitch, yaw);
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();
tf2::Quaternion q{ pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w };
std::cout.precision(3);
ROS_WARN_STREAM("object " << name << " has pose " << pose << "and RPY " << roll << ", " << pitch << ", "
<< yaw);
}
});
while (ros::ok())
{
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment