time_alignment_node.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
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 the Willow Garage 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 
36 
37 #include <ros/ros.h>
40 #include <calibration_msgs/JointStateCalibrationPattern.h>
41 #include <std_msgs/Float64.h>
42 #include <boost/thread/mutex.hpp>
43 
44 using namespace laser_joint_projector;
45 using namespace std;
46 
48 {
49 public:
51  {
52  cloud_num_ = 0;
53 
54  std::string robot_desc_string;
55  nh_.param("robot_description", robot_desc_string, string());
56  if (!kdl_parser::treeFromString(robot_desc_string, tree_))
57  ROS_FATAL("Failed to construct kdl tree");
58 
59  pub0_ = nh_.advertise<sensor_msgs::PointCloud>("cb_cloud_0", 1);
60  pub1_ = nh_.advertise<sensor_msgs::PointCloud>("cb_cloud_1", 1);
61  sub_features_ = nh_.subscribe("joint_state_features", 1, &TimeAlignmentNode::featuresCallback, this);
62  sub_offset_ = nh_.subscribe("keyboard_float", 1, &TimeAlignmentNode::offsetCallback, this);
63 
64  ros::NodeHandle private_nh_("~");
65 
66  private_nh_.param("root", root_, std::string("root_link"));
67  private_nh_.param("tip", tip_, std::string("tip_link"));
68 
69  projector_.configure(tree_, root_, tip_);
70  }
71 
72 private:
74  unsigned int cloud_num_;
81 
82  double offset_;
83  std::string root_;
84  std::string tip_;
85 
86  boost::mutex mutex_;
87 
88  calibration_msgs::JointStateCalibrationPatternConstPtr features_0_;
89  calibration_msgs::JointStateCalibrationPatternConstPtr features_1_;
90 
91  void featuresCallback(const calibration_msgs::JointStateCalibrationPatternConstPtr& msg)
92  {
93  boost::mutex::scoped_lock lock(mutex_);
94 
95  if (cloud_num_ == 0)
96  features_0_ = msg;
97  else if (cloud_num_ == 1)
98  features_1_ = msg;
99  else
100  ROS_FATAL("Cloud num is [%u]", cloud_num_);
101 
102  // Toggle the cloud #
103  cloud_num_ = 1 - cloud_num_;
104 
105  publish();
106  }
107 
108  void offsetCallback(const std_msgs::Float64ConstPtr& offset)
109  {
110  boost::mutex::scoped_lock lock(mutex_);
111  offset_ = offset->data;
112  publish();
113  }
114 
115  void publish()
116  {
117  ros::Duration offset(offset_);
118 
119  sensor_msgs::PointCloud cloud_0;
120  sensor_msgs::PointCloud cloud_1;
121 
122  if (features_0_)
123  {
124  cloud_0 = projector_.project(features_0_->joint_points, offset);
125  cloud_0.header.stamp = features_0_->header.stamp;
126  }
127 
128  if (features_1_)
129  {
130  cloud_1 = projector_.project(features_1_->joint_points, offset);
131  cloud_1.header.stamp = features_1_->header.stamp;
132  }
133 
134  cloud_0.header.frame_id = root_;
135  cloud_1.header.frame_id = root_;
136 
137  pub0_.publish(cloud_0);
138  pub1_.publish(cloud_1);
139  }
140 
141 };
142 
143 
144 int main(int argc, char** argv)
145 {
146  ros::init(argc, argv, "time_alignment");
147 
148  TimeAlignmentNode alignment_node;
149 
150  ros::spin();
151 
152  return 0;
153 }
#define ROS_FATAL(...)
calibration_msgs::JointStateCalibrationPatternConstPtr features_0_
void publish(const boost::shared_ptr< M > &message) const
void offsetCallback(const std_msgs::Float64ConstPtr &offset)
ros::Subscriber sub_features_
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ros::Subscriber sub_offset_
calibration_msgs::JointStateCalibrationPatternConstPtr features_1_
LaserJointProjector projector_
bool param(const std::string &param_name, T &param_val, const T &default_val) const
ROSCPP_DECL void spin()
KDL_PARSER_PUBLIC bool treeFromString(const std::string &xml, KDL::Tree &tree)
geometry_msgs::Point32 project(const std::map< std::string, double > &joint_map)
int main(int argc, char **argv)
void featuresCallback(const calibration_msgs::JointStateCalibrationPatternConstPtr &msg)


laser_joint_projector
Author(s): Vijay Pradeep
autogenerated on Tue Jun 1 2021 02:50:56