tf_publisher_capability.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2019, Hamburg University
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Hamburg University nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Jonas Tietz */
36 
41 #include <tf2_eigen/tf2_eigen.h>
44 
45 namespace move_group
46 {
47 TfPublisher::TfPublisher() : MoveGroupCapability("TfPublisher")
48 {
49 }
50 
52 {
53  keep_running_ = false;
54  thread_.join();
55 }
56 
57 namespace
58 {
59 void publishSubframes(tf2_ros::TransformBroadcaster& broadcaster, const moveit::core::FixedTransformsMap& subframes,
60  const std::string& parent_object, const ros::Time& stamp)
61 {
62  geometry_msgs::TransformStamped transform;
63  for (auto& subframe : subframes)
64  {
65  transform = tf2::eigenToTransform(subframe.second);
66  transform.child_frame_id = parent_object + "/" + subframe.first;
67  transform.header.stamp = stamp;
68  transform.header.frame_id = parent_object;
69  broadcaster.sendTransform(transform);
70  }
71 }
72 } // namespace
73 
75 {
77  geometry_msgs::TransformStamped transform;
78  ros::Rate rate(rate_);
79 
80  while (keep_running_)
81  {
82  {
84  planning_scene_monitor::LockedPlanningSceneRO locked_planning_scene(context_->planning_scene_monitor_);
85  collision_detection::WorldConstPtr world = locked_planning_scene->getWorld();
86  std::string planning_frame = locked_planning_scene->getPlanningFrame();
87 
88  for (const auto& obj : *world)
89  {
90  std::string object_frame = prefix_ + obj.second->id_;
91  transform = tf2::eigenToTransform(obj.second->pose_);
92  transform.child_frame_id = object_frame;
93  transform.header.stamp = stamp;
94  transform.header.frame_id = planning_frame;
95  broadcaster.sendTransform(transform);
96 
97  const moveit::core::FixedTransformsMap& subframes = obj.second->subframe_poses_;
98  publishSubframes(broadcaster, subframes, object_frame, stamp);
99  }
100 
101  const moveit::core::RobotState& rs = locked_planning_scene->getCurrentState();
102  std::vector<const moveit::core::AttachedBody*> attached_collision_objects;
103  rs.getAttachedBodies(attached_collision_objects);
104  for (const moveit::core::AttachedBody* attached_body : attached_collision_objects)
105  {
106  std::string object_frame = prefix_ + attached_body->getName();
107  transform = tf2::eigenToTransform(attached_body->getPose());
108  transform.child_frame_id = object_frame;
109  transform.header.stamp = stamp;
110  transform.header.frame_id = attached_body->getAttachedLinkName();
111  broadcaster.sendTransform(transform);
112 
113  const moveit::core::FixedTransformsMap& subframes = attached_body->getSubframes();
114  publishSubframes(broadcaster, subframes, object_frame, stamp);
115  }
116  }
117 
118  rate.sleep();
119  }
120 }
121 
123 {
125 
126  std::string prefix = nh.getNamespace() + "/";
127  nh.param("planning_scene_frame_publishing_rate", rate_, 10);
128  nh.param("planning_scene_tf_prefix", prefix_, prefix);
129  keep_running_ = true;
130 
131  ROS_INFO("Initializing MoveGroupTfPublisher with a frame publishing rate of %d", rate_);
132  thread_ = std::thread(&TfPublisher::publishPlanningSceneFrames, this);
133 }
134 } // namespace move_group
135 
move_group::TfPublisher::rate_
int rate_
Definition: tf_publisher_capability.h:118
move_group::MoveGroupCapability
Definition: move_group_capability.h:89
move_group::MoveGroupCapability::context_
MoveGroupContextPtr context_
Definition: move_group_capability.h:132
move_group::TfPublisher::TfPublisher
TfPublisher()
Definition: tf_publisher_capability.cpp:79
move_group::TfPublisher::keep_running_
bool keep_running_
Definition: tf_publisher_capability.h:121
move_group::TfPublisher::~TfPublisher
~TfPublisher() override
Definition: tf_publisher_capability.cpp:83
tf2_eigen.h
move_group::TfPublisher::thread_
std::thread thread_
Definition: tf_publisher_capability.h:120
moveit::core::RobotState
move_group::TfPublisher::initialize
void initialize() override
Definition: tf_publisher_capability.cpp:154
robot_state.h
transform
template Halfspace< double > transform(const Halfspace< double > &a, const Transform3< double > &tf)
obj
CollisionObject< S > * obj
transform_broadcaster.h
tf2::eigenToTransform
geometry_msgs::TransformStamped eigenToTransform(const Eigen::Affine3d &T)
tf_publisher_capability.h
moveit::core::AttachedBody
message_checks.h
move_group::TfPublisher::prefix_
std::string prefix_
Definition: tf_publisher_capability.h:119
tf2_ros::TransformBroadcaster::sendTransform
void sendTransform(const geometry_msgs::TransformStamped &transform)
moveit::core::RobotState::getAttachedBodies
void getAttachedBodies(std::vector< const AttachedBody * > &attached_bodies) const
moveit::core::FixedTransformsMap
std::map< std::string, Eigen::Isometry3d, std::less< std::string >, Eigen::aligned_allocator< std::pair< const std::string, Eigen::Isometry3d > > > FixedTransformsMap
move_group
Definition: capability_names.h:41
class_loader.hpp
ros::Time
attached_body.h
planning_scene_monitor::LockedPlanningSceneRO
tf2_ros::TransformBroadcaster
ros::NodeHandle::param
T param(const std::string &param_name, const T &default_val) const
ros::Rate
CLASS_LOADER_REGISTER_CLASS
CLASS_LOADER_REGISTER_CLASS(default_planner_request_adapters::AddIterativeSplineParameterization, planning_request_adapter::PlanningRequestAdapter)
ros::NodeHandle::getNamespace
const std::string & getNamespace() const
move_group::TfPublisher
Definition: tf_publisher_capability.h:76
ROS_INFO
#define ROS_INFO(...)
capability_names.h
ros::NodeHandle
ros::Time::now
static Time now()
move_group::TfPublisher::publishPlanningSceneFrames
void publishPlanningSceneFrames()
Definition: tf_publisher_capability.cpp:106


move_group
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Tue Oct 12 2021 02:25:39