laser_joint_projector_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 
38 #include <ros/console.h>
39 #include <ros/ros.h>
42 #include <calibration_msgs/JointStateCalibrationPattern.h>
43 
44 using namespace laser_joint_projector;
45 using namespace std;
46 
48  LaserJointProjector* projector,
49  const calibration_msgs::JointStateCalibrationPatternConstPtr& msg)
50 {
51  sensor_msgs::PointCloud cloud = projector->project(msg->joint_points, ros::Duration());
52  cloud.header.stamp = msg->header.stamp;
53  cloud.header.frame_id = "torso_lift_link";
54  pub->publish(cloud);
55 }
56 
57 int main(int argc, char** argv)
58 {
59  ros::init(argc, argv, "projector");
61 
62  KDL::Tree my_tree;
63  std::string robot_desc_string;
64  if (!n.getParam("robot_description", robot_desc_string))
65  ROS_FATAL("Couldn't get a robot_description from the param server");
66 
67  if (!kdl_parser::treeFromString(robot_desc_string, my_tree))
68  {
69  ROS_ERROR("Failed to construct kdl tree");
70  return false;
71  }
72 
73 // KDL::Tree my_tree;
74 // string filename= "/u/vpradeep/ros/pkgs/ros-pkg/stacks/robot_calibration/laser_joint_projector/pr2.urdf";
75 // if (!kdl_parser::treeFromFile(filename, my_tree))
76 // {
77 // ROS_ERROR("Failed to construct kdl tree");
78 // return -1;
79 // }
80 
81  LaserJointProjector projector;
82  projector.configure(my_tree, "torso_lift_link", "laser_tilt_link");
83 
84  // Output
85  ros::Publisher pub = n.advertise<sensor_msgs::PointCloud>("checkerboard_points", 1);
86 
87  // Input
88  boost::function<void (const calibration_msgs::JointStateCalibrationPatternConstPtr&)> cb
89  = boost::bind(&featuresCallback, &pub, &projector, _1);
90 
91  ros::Subscriber sub = n.subscribe(std::string("joint_state_features"), 1, cb);
92 
93  ros::spin();
94 
95  return 0;
96 }
int main(int argc, char **argv)
#define ROS_FATAL(...)
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void featuresCallback(ros::Publisher *pub, LaserJointProjector *projector, const calibration_msgs::JointStateCalibrationPatternConstPtr &msg)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
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)
void configure(const KDL::Tree &tree, const std::string &root, const std::string &tip)
bool getParam(const std::string &key, std::string &s) const
#define ROS_ERROR(...)


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