42 #include <calibration_msgs/JointStateCalibrationPattern.h> 49 const calibration_msgs::JointStateCalibrationPatternConstPtr& msg)
52 cloud.header.stamp = msg->header.stamp;
53 cloud.header.frame_id =
"torso_lift_link";
57 int main(
int argc,
char** argv)
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");
69 ROS_ERROR(
"Failed to construct kdl tree");
82 projector.
configure(my_tree,
"torso_lift_link",
"laser_tilt_link");
88 boost::function<void (const calibration_msgs::JointStateCalibrationPatternConstPtr&)> cb
int main(int argc, char **argv)
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)
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