40 #include <calibration_msgs/JointStateCalibrationPattern.h> 41 #include <std_msgs/Float64.h> 42 #include <boost/thread/mutex.hpp> 54 std::string robot_desc_string;
55 nh_.param(
"robot_description", robot_desc_string,
string());
57 ROS_FATAL(
"Failed to construct kdl tree");
59 pub0_ = nh_.advertise<sensor_msgs::PointCloud>(
"cb_cloud_0", 1);
60 pub1_ = nh_.advertise<sensor_msgs::PointCloud>(
"cb_cloud_1", 1);
66 private_nh_.
param(
"root", root_, std::string(
"root_link"));
67 private_nh_.
param(
"tip", tip_, std::string(
"tip_link"));
69 projector_.configure(tree_, root_, tip_);
88 calibration_msgs::JointStateCalibrationPatternConstPtr
features_0_;
89 calibration_msgs::JointStateCalibrationPatternConstPtr
features_1_;
91 void featuresCallback(
const calibration_msgs::JointStateCalibrationPatternConstPtr& msg)
93 boost::mutex::scoped_lock lock(mutex_);
97 else if (cloud_num_ == 1)
100 ROS_FATAL(
"Cloud num is [%u]", cloud_num_);
103 cloud_num_ = 1 - cloud_num_;
110 boost::mutex::scoped_lock lock(mutex_);
111 offset_ = offset->data;
119 sensor_msgs::PointCloud cloud_0;
120 sensor_msgs::PointCloud cloud_1;
124 cloud_0 = projector_.
project(features_0_->joint_points, offset);
125 cloud_0.header.stamp = features_0_->header.stamp;
130 cloud_1 = projector_.
project(features_1_->joint_points, offset);
131 cloud_1.header.stamp = features_1_->header.stamp;
134 cloud_0.header.frame_id = root_;
135 cloud_1.header.frame_id = root_;
144 int main(
int argc,
char** argv)
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 ¶m_name, T ¶m_val, const T &default_val) const
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)