From 873fc5cde2bbbeb1dc543db9d41512538935cf4c Mon Sep 17 00:00:00 2001
From: Jennifer Buehler <jennifer.e.buehler@gmail.com>
Date: Sun, 6 Mar 2016 23:28:31 +0100
Subject: [PATCH] added tf object broadcaster and generation of simple grasp
message
---
Dockerfile | 1 +
object_msgs/CMakeLists.txt | 2 +-
object_msgs/msg/Object.msg | 49 ++++-
object_msgs/msg/ObjectPose.msg | 2 +-
object_msgs/srv/RegisterObject.srv | 16 ++
object_msgs_tools/CMakeLists.txt | 207 ++++++++++++++++++
.../config/ObjectTFBroadcaster.yaml | 26 +++
.../object_msgs_tools/ObjectFunctions.h | 57 +++++
.../object_msgs_tools/ObjectTFBroadcaster.h | 119 ++++++++++
.../launch/object_tf_broadcaster.launch | 5 +
object_msgs_tools/package.xml | 56 +++++
object_msgs_tools/src/ObjectFunctions.cpp | 78 +++++++
object_msgs_tools/src/ObjectTFBroadcaster.cpp | 171 +++++++++++++++
.../src/object_tf_broadcaster_node.cpp | 50 +++++
.../src/register_object_client.cpp | 32 +++
15 files changed, 866 insertions(+), 5 deletions(-)
create mode 100644 object_msgs/srv/RegisterObject.srv
create mode 100644 object_msgs_tools/CMakeLists.txt
create mode 100644 object_msgs_tools/config/ObjectTFBroadcaster.yaml
create mode 100644 object_msgs_tools/include/object_msgs_tools/ObjectFunctions.h
create mode 100644 object_msgs_tools/include/object_msgs_tools/ObjectTFBroadcaster.h
create mode 100644 object_msgs_tools/launch/object_tf_broadcaster.launch
create mode 100644 object_msgs_tools/package.xml
create mode 100644 object_msgs_tools/src/ObjectFunctions.cpp
create mode 100644 object_msgs_tools/src/ObjectTFBroadcaster.cpp
create mode 100644 object_msgs_tools/src/object_tf_broadcaster_node.cpp
create mode 100644 object_msgs_tools/src/register_object_client.cpp
diff --git a/Dockerfile b/Dockerfile
index 0d4ef3e..a2cc03b 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -20,6 +20,7 @@ RUN apt-get update && apt-get install -y \
&& rm -rf /var/lib/apt/lists/
COPY object_msgs /catkin_ws/src/object_msgs
+COPY object_msgs_tools /catkin_ws/src/object_msgs_tools
COPY path_navigation_msgs /catkin_ws/src/path_navigation_msgs
# Build
diff --git a/object_msgs/CMakeLists.txt b/object_msgs/CMakeLists.txt
index 2044542..73ffafe 100644
--- a/object_msgs/CMakeLists.txt
+++ b/object_msgs/CMakeLists.txt
@@ -57,7 +57,7 @@ add_message_files(
## Generate services in the 'srv' folder
add_service_files(
DIRECTORY srv
- FILES ObjectInfo.srv
+ FILES ObjectInfo.srv RegisterObject.srv
)
## Generate actions in the 'action' folder
diff --git a/object_msgs/msg/Object.msg b/object_msgs/msg/Object.msg
index 2794848..3551131 100644
--- a/object_msgs/msg/Object.msg
+++ b/object_msgs/msg/Object.msg
@@ -1,7 +1,10 @@
# a header, used for interpreting the poses
Header header
-# the name of the object
+# the name of the object. This should usually
+# coincide with the /tf frame of the base/origin
+# of the object, though this is no mandatory
+# requirement.
string name
# If known: The object type in a database of known objects
@@ -21,12 +24,52 @@ geometry_msgs/Pose[] mesh_poses
shape_msgs/Plane[] planes
geometry_msgs/Pose[] plane_poses
+# Index into the primitive_poses to the
+# pose which is considered to be the
+# object's root / origin. Can also be
+# set to constants ORIGIN_AVERAGE,
+# ORIGIN_UNDEFINED or ORIGIN_CUSTOM.
+int32 primitive_origin
+
+# Index into the mesh_poses to the
+# pose which is considered to be the
+# object's root / origin. Can also be
+# set to constants ORIGIN_AVERAGE,
+# ORIGIN_UNDEFINED or ORIGIN_CUSTOM.
+int32 mesh_origin
+
+# If 'mesh_origin' or 'primitive_origin'
+# are set to ORIGIN_CUSTOM, this field
+# contains the origin of the overall object.
+geometry_msgs/Pose origin
+
+# Information in the message, either SHAPE or POSE
+byte content
+
+# For field 'content':
# Message contains the shape(s) of the object including poses
byte SHAPE=0
+# For field 'content':
# Message contains only the pose of the primitives,
# i.e. only *_poses will be with values.
byte POSE=1
-# Information in the message, either SHAPE or POSE
-byte content
+# For fields 'mesh_origin' and 'primitives_origin':
+# the root/origin of the object is the
+# average of all poses.
+# Because the average of several quaternions
+# can only be determined approximately when
+# the quaternions are close together, this
+# setting requires at least the orientation part
+# of the field 'origin' to be set in addition.
+int32 ORIGIN_AVERAGE=-1
+
+# For fields 'mesh_origin' and 'primitives_origin':
+# The origin of the object cannot be defined by this
+# set of poses.
+int32 ORIGIN_UNDEFINED=-2
+
+# For fields 'mesh_origin' and 'primitives_origin':
+# The origin is given in the custom field 'origin'
+int32 ORIGIN_CUSTOM=-3
diff --git a/object_msgs/msg/ObjectPose.msg b/object_msgs/msg/ObjectPose.msg
index cb20c21..3962ab1 100644
--- a/object_msgs/msg/ObjectPose.msg
+++ b/object_msgs/msg/ObjectPose.msg
@@ -1,6 +1,6 @@
# the name of the object
string name
-# thep ose of the object
+# the pose of the object
geometry_msgs/PoseStamped pose
diff --git a/object_msgs/srv/RegisterObject.srv b/object_msgs/srv/RegisterObject.srv
new file mode 100644
index 0000000..76fe1d6
--- /dev/null
+++ b/object_msgs/srv/RegisterObject.srv
@@ -0,0 +1,16 @@
+# object name to be registered
+string name
+
+---
+
+# succes flag
+byte success
+
+# successfully registered
+byte SUCCESS=0
+
+# object already registered
+byte EXISTS=1
+
+# could not query required information for this object
+byte ERROR_INFO=2
diff --git a/object_msgs_tools/CMakeLists.txt b/object_msgs_tools/CMakeLists.txt
new file mode 100644
index 0000000..fa5f7b1
--- /dev/null
+++ b/object_msgs_tools/CMakeLists.txt
@@ -0,0 +1,207 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(object_msgs_tools)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+ object_msgs
+ roscpp
+ eigen_conversions
+ tf
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+## * add a build_depend tag for "message_generation"
+## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+## but can be declared for certainty nonetheless:
+## * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+## * add "message_generation" and every package in MSG_DEP_SET to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * add "message_runtime" and every package in MSG_DEP_SET to
+## catkin_package(CATKIN_DEPENDS ...)
+## * uncomment the add_*_files sections below as needed
+## and list every .msg/.srv/.action file to be processed
+## * uncomment the generate_messages entry below
+## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+# FILES
+# Message1.msg
+# Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+# FILES
+# Service1.srv
+# Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+# FILES
+# Action1.action
+# Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+# DEPENDENCIES
+# object_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+## * add a build_depend and a run_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+## * add "dynamic_reconfigure" to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * uncomment the "generate_dynamic_reconfigure_options" section below
+## and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+# cfg/DynReconf1.cfg
+# cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+ INCLUDE_DIRS include
+ LIBRARIES object_msgs_tools
+ CATKIN_DEPENDS object_msgs roscpp eigen_conversions tf
+# DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+ include
+ ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+add_library(object_msgs_tools
+ src/ObjectTFBroadcaster.cpp
+ src/ObjectFunctions.cpp
+)
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+add_dependencies(object_msgs_tools ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+add_executable(object_tf_broadcaster_node src/object_tf_broadcaster_node.cpp)
+add_executable(register_object_client src/register_object_client.cpp)
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+add_dependencies(object_tf_broadcaster_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(register_object_client ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(object_msgs_tools
+ ${catkin_LIBRARIES}
+)
+target_link_libraries(object_tf_broadcaster_node
+ object_msgs_tools
+ ${catkin_LIBRARIES}
+)
+target_link_libraries(register_object_client
+ ${catkin_LIBRARIES}
+)
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+# scripts/my_python_script
+# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+install(TARGETS object_msgs_tools object_tf_broadcaster_node register_object_client
+ ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+## Mark cpp header files for installation
+install(DIRECTORY include/${PROJECT_NAME}/
+ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+ FILES_MATCHING PATTERN "*.h"
+ PATTERN ".git" EXCLUDE
+)
+
+install(DIRECTORY launch/
+ DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ FILES_MATCHING PATTERN "*.launch"
+ PATTERN ".git" EXCLUDE
+)
+
+
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+# # myfile1
+# # myfile2
+# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_object_msgs_tools.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
diff --git a/object_msgs_tools/config/ObjectTFBroadcaster.yaml b/object_msgs_tools/config/ObjectTFBroadcaster.yaml
new file mode 100644
index 0000000..b161def
--- /dev/null
+++ b/object_msgs_tools/config/ObjectTFBroadcaster.yaml
@@ -0,0 +1,26 @@
+# the topic at which object_msgs/Object information is being
+# published. Can be left empty to instead rely on object_service
+object_topic: ""
+
+# the name under which a service of type object_msgs/RegisterObject
+# is going to be provided which can be used to add objects to be published wiht /tf.
+register_object_service: "/object_tf_broadcaster/register_object"
+
+# the object_msgs/ObjectInfo service where information
+# about objects can be queried. Can be left empty to rely on messages being
+# published on object_topic. The service is queried at query_object_info_rate.
+object_service: "/gazebo_world/object_info"
+
+# rate at which to publish information to /tf. This may differ
+# from the query_object_info_rate, i.e. it may be set higer, in order
+# to keep refreshing the timestamps of the /tf tree. Because it is expected
+# that objects don't change their state very often, query_object_info_rate
+# (or the rate at which object messages arrive at object_topic) can be lower.
+publish_tf_rate: 10
+
+# when using the object_msgs/ObjectInfo service
+# to obtain object information, this is the rate at which this is done.
+# If no service is to be used, and instead it should be relied on
+# messages published on object_topic only, then object_service
+# should be left empty, and query_object_info_rate will be ignored.
+query_object_info_rate: 1
diff --git a/object_msgs_tools/include/object_msgs_tools/ObjectFunctions.h b/object_msgs_tools/include/object_msgs_tools/ObjectFunctions.h
new file mode 100644
index 0000000..af76dff
--- /dev/null
+++ b/object_msgs_tools/include/object_msgs_tools/ObjectFunctions.h
@@ -0,0 +1,57 @@
+#ifndef OBJECT_MSGS_TOOLS_OBJECTFUNCTIONS_H
+#define OBJECT_MSGS_TOOLS_OBJECTFUNCTIONS_H
+
+#include <object_msgs/Object.h>
+#include <geometry_msgs/PoseStamped.h>
+
+namespace object_msgs_tools {
+
+/**
+ * Provides a collection of convenience function for
+ * object_msgs/Object.
+ *
+ * \author Jennifer Buehler
+ * \date March 2016
+ */
+class ObjectFunctions
+{
+public:
+
+ ObjectFunctions(){}
+
+ /**
+ * Get the object pose out of the information in \e object according to what's specified
+ * in the message (fields object.primitive_origin and/or object.mesh_origin).
+ * If both mesh and primitive origins are enabled, the primitives are used to determine
+ * the object pose.
+ * \return false if insufficient information given to determine object pose, true if
+ * pose is returned in \e pose.
+ */
+ static bool getObjectPose(const object_msgs::Object& object, geometry_msgs::PoseStamped& pose);
+
+private:
+
+ /**
+ * \param idx the value of either Object::primitive_origin or Object::mesh_origin
+ * \param object_header the header of the Object message.
+ * \param poses the poses, either Object::primitive_poses or Object::mesh_poses
+ * \param origin the field Object::origin, in case it is required.
+ */
+ static bool getPoseFromFields(const std_msgs::Header& object_header, int idx,
+ const std::vector<geometry_msgs::Pose>& poses,
+ const geometry_msgs::Pose & origin,
+ geometry_msgs::PoseStamped& pose);
+
+ /**
+ * Computes the average pose of several positions.
+ * Because the average of several quaternions
+ * can only be determined approximately when
+ * the quaternions are close together, the orientation
+ * parts are ignored, and only the position is returned.
+ */
+ static geometry_msgs::Point getAveragePointFrom(const std::vector<geometry_msgs::Pose>& poses);
+};
+
+}
+
+#endif // OBJECT_MSGS_TOOLS_OBJECTFUNCTIONS_H
diff --git a/object_msgs_tools/include/object_msgs_tools/ObjectTFBroadcaster.h b/object_msgs_tools/include/object_msgs_tools/ObjectTFBroadcaster.h
new file mode 100644
index 0000000..50c9bce
--- /dev/null
+++ b/object_msgs_tools/include/object_msgs_tools/ObjectTFBroadcaster.h
@@ -0,0 +1,119 @@
+#ifndef OBJECT_MSGS_TOOLS_OBJECTTFBROADCASTER_H
+#define OBJECT_MSGS_TOOLS_OBJECTTFBROADCASTER_H
+
+#include <ros/ros.h>
+#include <object_msgs/Object.h>
+#include <object_msgs/RegisterObject.h>
+#include <tf/transform_broadcaster.h>
+#include <boost/thread.hpp>
+
+namespace object_msgs_tools {
+
+/**
+ * Subscribes to object_msgs/Object topic or uses the object_msgs/ObjectInfo service
+ * to query information about objects, and then broadcasts the information as /tf transform.
+ * Only objects which have been explicitly registered with registerObject() are processed.
+ * Only objects which arrive or are queried with the service and have a Object.header.frame_id
+ * which is **not empty** are considered valid and are processed.
+ *
+ * Refer to the constructor for more information about how it works.
+ *
+ * \author Jennifer Buehler
+ * \date March 2016
+ */
+class ObjectTFBroadcaster
+{
+public:
+
+ /**
+ * Constructor. Parameters describe how the ObjectTFBRoadcaster works.
+ * \param register_object_service the name under which a service of type object_msgs/RegisterObject
+ * is going to be provided which can be used to add objects to be published wiht /tf.
+ * \param query_object_info_rate when using the object_msgs/ObjectInfo service
+ * to obtain object information, this is the rate at which this is done.
+ * If no service is to be used, and instead it should be relied on
+ * messages published on \e object_topic only, then \e object_service
+ * should be left empty, and \e query_object_info_rate will be ignored.
+ * \param publish_tf_rate rate at which to publish information to /tf. This may differ
+ * from the \e query_object_info_rate, i.e. it may be set higer, in order
+ * to keep refreshing the timestamps of the /tf tree. Because it is expected
+ * that objects don't change their state very often, \e query_object_info_rate
+ * (or the rate at which object messages arrive at \e object_topic) can be lower.
+ * \param object_topic the topic at which object_msgs/Object information is being
+ * published. Can be left empty to instead rely on \e object_service
+ * \param object_service the object_msgs/ObjectInfo service where information
+ * about objects can be queried. Can be left empty to rely on messages being
+ * published on \e object_topic. The service is queried at \e query_object_info_rate.
+ */
+ ObjectTFBroadcaster(
+ ros::NodeHandle& n,
+ const std::string& register_object_service,
+ unsigned int publish_tf_rate,
+ unsigned int query_object_info_rate,
+ const std::string& object_topic,
+ const std::string& object_service);
+
+ bool isRegistered(const std::string& name) const;
+
+ /**
+ * return code as in object_msgs::RegisterObject
+ */
+ int registerObject(const std::string& name, bool printMsgs = true);
+
+private:
+
+ /**
+ * service callback
+ */
+ bool registerObjectService(object_msgs::RegisterObject::Request &req,
+ object_msgs::RegisterObject::Response &res);
+
+ /**
+ * Uses the service to request object information (only pose)
+ */
+ bool queryObjectPose(const std::string& name, object_msgs::Object& obj, bool printErrors = true);
+
+ /**
+ * This transform (pose) holds for the target frame frame_name
+ */
+ void sendTF(const geometry_msgs::PoseStamped& pose, const std::string& frame_name);
+
+ /**
+ * Updates information of this object in the map.
+ */
+ bool updateObject(const object_msgs::Object& obj);
+
+ /**
+ * Callback for subscriber to object_msgs/Object
+ */
+ void objectCallback(const object_msgs::Object& obj);
+
+ /**
+ * Event loop for publishing tf
+ */
+ void publishTFEvent(const ros::TimerEvent& e);
+
+ /**
+ * Event loop for querying the newest object information
+ */
+ void queryObjectPoseEvent(const ros::TimerEvent& e);
+
+ // all object poses which have been registered
+ // and will be continuously re-published on /tf.
+ std::map<std::string, geometry_msgs::PoseStamped> obj_poses;
+ mutable boost::recursive_mutex obj_poses_mutex;
+
+ ros::ServiceServer register_object_srv;
+ ros::ServiceClient object_info_srv;
+ ros::Subscriber object_info_sub;
+
+ ros::NodeHandle node;
+
+ tf::TransformBroadcaster tf_broadcaster;
+ ros::Timer publish_tf_timer;
+ ros::Timer query_object_info_timer;
+};
+
+}
+
+#endif // OBJECT_MSGS_TOOLS_OBJECTTFBROADCASTER_H
diff --git a/object_msgs_tools/launch/object_tf_broadcaster.launch b/object_msgs_tools/launch/object_tf_broadcaster.launch
new file mode 100644
index 0000000..ed61d24
--- /dev/null
+++ b/object_msgs_tools/launch/object_tf_broadcaster.launch
@@ -0,0 +1,5 @@
+<launch>
+ <arg name="config_file" default="$(find object_msgs_tools)/config/ObjectTFBroadcaster.yaml"/>
+ <rosparam ns="object_tf_broadcaster" file="$(arg config_file)"/>
+ <node name="object_tf_broadcaster" pkg="object_msgs_tools" type="object_tf_broadcaster_node" args="" output="screen" respawn="true"/>
+</launch>
diff --git a/object_msgs_tools/package.xml b/object_msgs_tools/package.xml
new file mode 100644
index 0000000..038e84a
--- /dev/null
+++ b/object_msgs_tools/package.xml
@@ -0,0 +1,56 @@
+<?xml version="1.0"?>
+<package>
+ <name>object_msgs_tools</name>
+ <version>0.0.0</version>
+ <description>The object_msgs_tools package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <maintainer email="jennifer.e.buehler@gmail.com">Jennifer Buehler</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>GPLv3</license>
+
+
+ <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/object_msgs_tools</url> -->
+
+
+ <!-- Author tags are optional, mutiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintianers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+ <!-- The *_depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use run_depend for packages you need at runtime: -->
+ <!-- <run_depend>message_runtime</run_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>object_msgs</build_depend>
+ <build_depend>roscpp</build_depend>
+ <build_depend>eigen_conversions</build_depend>
+ <build_depend>tf</build_depend>
+ <run_depend>object_msgs</run_depend>
+ <run_depend>roscpp</run_depend>
+ <run_depend>eigen_conversions</run_depend>
+ <run_depend>tf</run_depend>
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
diff --git a/object_msgs_tools/src/ObjectFunctions.cpp b/object_msgs_tools/src/ObjectFunctions.cpp
new file mode 100644
index 0000000..5650657
--- /dev/null
+++ b/object_msgs_tools/src/ObjectFunctions.cpp
@@ -0,0 +1,78 @@
+#include <ros/ros.h>
+#include <object_msgs_tools/ObjectFunctions.h>
+#include <eigen_conversions/eigen_msg.h>
+
+using object_msgs_tools::ObjectFunctions;
+
+bool ObjectFunctions::getObjectPose(const object_msgs::Object& object, geometry_msgs::PoseStamped& pose)
+{
+ if ((object.primitive_origin == object_msgs::Object::ORIGIN_UNDEFINED) &&
+ (object.mesh_origin == object_msgs::Object::ORIGIN_UNDEFINED))
+ {
+ return false;
+ }
+
+ bool success = false;
+ if (object.primitive_origin != object_msgs::Object::ORIGIN_UNDEFINED)
+ {
+ success = getPoseFromFields(object.header,object.primitive_origin,
+ object.primitive_poses, object.origin, pose);
+ }
+ else // the mesh origin must be defined because of check above
+ {
+ success = getPoseFromFields(object.header,object.mesh_origin,
+ object.mesh_poses, object.origin, pose);
+ }
+ return success;
+}
+
+bool ObjectFunctions::getPoseFromFields(const std_msgs::Header& object_header, int idx,
+ const std::vector<geometry_msgs::Pose>& poses,
+ const geometry_msgs::Pose & origin,
+ geometry_msgs::PoseStamped& pose)
+{
+ if (idx == object_msgs::Object::ORIGIN_UNDEFINED)
+ return false;
+
+ if (idx == object_msgs::Object::ORIGIN_AVERAGE)
+ {
+ pose.pose.position = getAveragePointFrom(poses);
+ pose.pose.orientation = origin.orientation;
+ }
+ else if (idx == object_msgs::Object::ORIGIN_CUSTOM)
+ {
+ pose.pose = origin;
+ }
+ else if (idx > 0)
+ {
+ if (poses.size() <= idx)
+ {
+ ROS_ERROR_STREAM("ObjectFunctions: Inconsistent object, "<<
+ "has less primitive poses than required (" <<
+ poses.size()<<", required "<<idx <<")");
+ return false;
+ }
+ pose.pose = poses[idx];
+ }
+ else
+ {
+ ROS_ERROR_STREAM("Unknown mode of Object::primitive_origin or Object::mesh_origin: "<<idx);
+ return false;
+ }
+ pose.header = object_header;
+ return true;
+}
+geometry_msgs::Point ObjectFunctions::getAveragePointFrom(const std::vector<geometry_msgs::Pose>& poses)
+{
+ Eigen::Vector3d p;
+ for (int i=0; i < poses.size(); ++i)
+ {
+ Eigen::Vector3d _p;
+ const geometry_msgs::Pose& pi = poses[i];
+ tf::pointMsgToEigen(pi.position,_p);
+ p+=_p;
+ }
+ geometry_msgs::Point ret;
+ tf::pointEigenToMsg(p,ret);
+ return ret;
+}
diff --git a/object_msgs_tools/src/ObjectTFBroadcaster.cpp b/object_msgs_tools/src/ObjectTFBroadcaster.cpp
new file mode 100644
index 0000000..fc0a227
--- /dev/null
+++ b/object_msgs_tools/src/ObjectTFBroadcaster.cpp
@@ -0,0 +1,171 @@
+#include <object_msgs_tools/ObjectTFBroadcaster.h>
+#include <object_msgs_tools/ObjectFunctions.h>
+#include <object_msgs/ObjectInfo.h>
+
+using object_msgs_tools::ObjectTFBroadcaster;
+
+ObjectTFBroadcaster::ObjectTFBroadcaster(
+ ros::NodeHandle& n,
+ const std::string& register_object_service,
+ unsigned int publish_tf_rate,
+ unsigned int query_object_info_rate,
+ const std::string& object_topic,
+ const std::string& object_service):
+ node(n)
+{
+ if (object_service.empty() && object_topic.empty())
+ {
+ ROS_ERROR("Can't use ObjetTFBroadcaster if neither service nor object info topic is set");
+ throw std::runtime_error("Can't use ObjetTFBroadcaster if neither service nor object info topic is set");
+ }
+
+ ros::Rate rate(publish_tf_rate);
+ publish_tf_timer = node.createTimer(rate, &ObjectTFBroadcaster::publishTFEvent, this);
+ if (!register_object_service.empty())
+ {
+ ROS_INFO_STREAM("Advertising service "<<register_object_service);
+ register_object_srv = node.advertiseService(register_object_service, &ObjectTFBroadcaster::registerObjectService,this);
+ }
+
+ if (!object_service.empty())
+ {
+ ros::Rate qRate(query_object_info_rate);
+ query_object_info_timer=node.createTimer(qRate,&ObjectTFBroadcaster::queryObjectPoseEvent, this);
+ query_object_info_timer.start();
+ object_info_srv = node.serviceClient<object_msgs::ObjectInfo>(object_service);
+ }
+ if (!object_topic.empty())
+ object_info_sub = node.subscribe(object_topic,1000,&ObjectTFBroadcaster::objectCallback, this);
+}
+
+bool ObjectTFBroadcaster::isRegistered(const std::string& name) const
+{
+ obj_poses_mutex.lock();
+ bool exists= obj_poses.find(name) != obj_poses.end();
+ obj_poses_mutex.unlock();
+ return exists;
+}
+
+bool ObjectTFBroadcaster::registerObjectService(object_msgs::RegisterObject::Request &req,
+ object_msgs::RegisterObject::Response &res)
+{
+ ROS_INFO_STREAM("Calling ObjectTFBroadcaster service with "<<req.name);
+ res.success = registerObject(req.name, true);
+ // always return success because none of the errors
+ // are fatal, the object will probably still be updated
+ // even if the information was not available at the time.
+ // It was still added (or already exists).
+ return true;
+}
+
+
+int ObjectTFBroadcaster::registerObject(const std::string& name, bool printMsgs)
+{
+ object_msgs::Object obj;
+ // Get initial information of the object.
+ // If this fails it doesn't matter, the first pose inserted will be invalid
+ queryObjectPose(name,obj, false);
+ geometry_msgs::PoseStamped pose;
+ if (!ObjectFunctions::getObjectPose(obj, pose)){
+ if (printMsgs) ROS_WARN_STREAM("ObjectTFBroadcaster: Could not get pose for object '"<<name<<"'");
+ return object_msgs::RegisterObject::Response::ERROR_INFO;
+ }
+
+ obj_poses_mutex.lock();
+ bool success = obj_poses.insert(std::make_pair(name,pose)).second;
+ obj_poses_mutex.unlock();
+ if (!success)
+ {
+ if (printMsgs)
+ ROS_WARN_STREAM("Object " << name <<
+ " could not be added in ObjectTFBroadcaster because it was already registered.");
+ return object_msgs::RegisterObject::Response::EXISTS;
+ }
+ return object_msgs::RegisterObject::Response::SUCCESS;
+}
+
+
+bool ObjectTFBroadcaster::queryObjectPose(const std::string& name, object_msgs::Object& obj, bool printErrors)
+{
+ if (object_info_srv.getService().empty())
+ {
+ if (printErrors) ROS_ERROR("ObjectTFBroadcaster: Service to request object_msgs/Object is not running.");
+ return false;
+ }
+ object_msgs::ObjectInfo srv;
+ srv.request.name = name;
+ srv.request.get_geometry = false;
+ if (object_info_srv.call(srv)){
+ //ROS_INFO("Result:");
+ //std::cout<<srv.response<<std::endl;
+ }else{
+ if (printErrors) ROS_ERROR("ObjectTFBroadcaster: Failed to call service to obtain object info");
+ return false;
+ }
+ obj=srv.response.object;
+ return true;
+}
+
+void ObjectTFBroadcaster::sendTF(const geometry_msgs::PoseStamped& pose, const std::string& frame_name){
+ tf::Transform transform;
+ transform.setOrigin( tf::Vector3(pose.pose.position.x, pose.pose.position.y, pose.pose.position.z) );
+ tf::Quaternion q(pose.pose.orientation.x,pose.pose.orientation.y,pose.pose.orientation.z,pose.pose.orientation.w);
+ transform.setRotation(q);
+ // ROS_INFO_STREAM("Send tf in "<<frame_name<<". "); //<<std::endl<<pose);
+ tf_broadcaster.sendTransform(tf::StampedTransform(transform, pose.header.stamp, pose.header.frame_id, frame_name));
+ // ROS_INFO("TF sent.");
+}
+
+
+bool ObjectTFBroadcaster::updateObject(const object_msgs::Object& obj)
+{
+ // ROS_INFO_STREAM("ObjectTFBroadcaster: Got object "<<obj);
+ geometry_msgs::PoseStamped pose;
+ if (!ObjectFunctions::getObjectPose(obj, pose)){
+ ROS_ERROR("ObjectTFBroadcaster: Could not get pose");
+ return false;
+ }
+ std::map<std::string, geometry_msgs::PoseStamped>::iterator it;
+ obj_poses_mutex.lock();
+ it = obj_poses.find(obj.name);
+ bool success = (it != obj_poses.end());
+ if (success) it->second=pose;
+ obj_poses_mutex.unlock();
+ if (!success)
+ {
+ ROS_ERROR_STREAM("ObjectTFBroadcaster: Could not update object "
+ <<obj.name<<" because it was not registered. Call registerObject() to add it.");
+ }
+ return success;
+}
+
+void ObjectTFBroadcaster::objectCallback(const object_msgs::Object& obj){
+ updateObject(obj);
+}
+
+void ObjectTFBroadcaster::publishTFEvent(const ros::TimerEvent& e){
+ // ROS_INFO("TF");
+ obj_poses_mutex.lock();
+ std::map<std::string, geometry_msgs::PoseStamped>::iterator it;
+ for (it = obj_poses.begin(); it!=obj_poses.end(); ++it)
+ {
+ it->second.header.stamp=ros::Time::now();
+ // ROS_INFO_STREAM("Sending TF for "<<it->first);//<<": "<<it->second);
+ sendTF(it->second,it->first);
+ }
+ obj_poses_mutex.unlock();
+}
+
+void ObjectTFBroadcaster::queryObjectPoseEvent(const ros::TimerEvent& e)
+{
+ // ROS_INFO("Query");
+ obj_poses_mutex.lock();
+ std::map<std::string, geometry_msgs::PoseStamped>::iterator it;
+ for (it = obj_poses.begin(); it!=obj_poses.end(); ++it)
+ {
+ object_msgs::Object obj;
+ if (!queryObjectPose(it->first,obj,true)) continue;
+ updateObject(obj);
+ }
+ obj_poses_mutex.unlock();
+}
diff --git a/object_msgs_tools/src/object_tf_broadcaster_node.cpp b/object_msgs_tools/src/object_tf_broadcaster_node.cpp
new file mode 100644
index 0000000..07b5cf5
--- /dev/null
+++ b/object_msgs_tools/src/object_tf_broadcaster_node.cpp
@@ -0,0 +1,50 @@
+#include <object_msgs_tools/ObjectTFBroadcaster.h>
+
+/***
+ * Launches a node which runs the ObjectTFBroadcaster.
+ *
+ * \author Jennifer Buehler
+ * \date March 2016
+ */
+int main(int argc, char**argv){
+ ros::init(argc, argv, "object_tf_broadcaster");
+
+ ros::NodeHandle priv("~");
+ ros::NodeHandle pub("");
+
+ // use same namespace which is used by launch file to load ROS params
+ ros::NodeHandle paramNH("object_tf_broadcaster");
+
+ std::string REGISTER_OBJECT_SERVICE="/register_object";
+ paramNH.param<std::string>("register_object_service", REGISTER_OBJECT_SERVICE, REGISTER_OBJECT_SERVICE);
+
+ std::string OBJECT_TOPIC="/gazebo_world/object";
+ paramNH.param<std::string>("object_topic", OBJECT_TOPIC, OBJECT_TOPIC);
+
+ std::string OBJECT_SERVICE="/gazebo_world/object_info";
+ paramNH.param<std::string>("object_service", OBJECT_SERVICE, OBJECT_SERVICE);
+
+ int PUBLISH_TF_RATE=10;
+ paramNH.param<int>("publish_tf_rate", PUBLISH_TF_RATE, PUBLISH_TF_RATE);
+
+ int QUERY_OBJECT_INFO_RATE=1;
+ paramNH.param<int>("query_object_info_rate", QUERY_OBJECT_INFO_RATE, QUERY_OBJECT_INFO_RATE);
+
+ ROS_INFO_STREAM("Starting ObjectTFBroadcaster with topics: register="<<REGISTER_OBJECT_SERVICE
+ <<", object="<<OBJECT_TOPIC<<", object service="<<OBJECT_SERVICE<<" tf rate ="
+ <<PUBLISH_TF_RATE<<", query info rate: "<<QUERY_OBJECT_INFO_RATE);
+
+ object_msgs_tools::ObjectTFBroadcaster broadcaster(
+ pub,
+ REGISTER_OBJECT_SERVICE,
+ PUBLISH_TF_RATE,
+ QUERY_OBJECT_INFO_RATE,
+ OBJECT_TOPIC,
+ OBJECT_SERVICE);
+
+ // ros::MultiThreadedSpinner spinner(4); // Use 4 threads
+ // spinner.spin(); // spin() will not return until the node has been shutdown
+ ros::spin();
+ ROS_INFO("Exit ObjectTFBroadcater");
+ return 0;
+}
diff --git a/object_msgs_tools/src/register_object_client.cpp b/object_msgs_tools/src/register_object_client.cpp
new file mode 100644
index 0000000..5b7f126
--- /dev/null
+++ b/object_msgs_tools/src/register_object_client.cpp
@@ -0,0 +1,32 @@
+#include <ros/ros.h>
+#include <object_msgs/RegisterObject.h>
+
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv, "register_object_client");
+ ros::NodeHandle priv("~");
+ ros::NodeHandle pub("");
+
+ if (!pub.hasParam("/object_tf_broadcaster/register_object_service"))
+ {
+ ROS_ERROR("Missing parameter register_object_service");
+ return 0;
+ }
+ std::string REGISTER_OBJECT_SERVICE;
+ priv.param<std::string>("/object_tf_broadcaster/register_object_service", REGISTER_OBJECT_SERVICE, REGISTER_OBJECT_SERVICE);
+ ROS_INFO("Using register object service name: <%s>", REGISTER_OBJECT_SERVICE.c_str());
+
+ ros::ServiceClient client = pub.serviceClient<object_msgs::RegisterObject>(REGISTER_OBJECT_SERVICE);
+ object_msgs::RegisterObject srv;
+ srv.request.name = argv[1];
+ if (client.call(srv))
+ {
+ ROS_INFO("Result:");
+ std::cout<<srv.response<<std::endl;
+ }
+ else
+ {
+ ROS_ERROR("Failed to call service %s",REGISTER_OBJECT_SERVICE.c_str());
+ }
+ return 0;
+}
--
GitLab