Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00036
00037
00038 #include <ros/console.h>
00039 #include <ros/ros.h>
00040 #include <laser_joint_projector/laser_joint_projector.h>
00041 #include <kdl_parser/kdl_parser.hpp>
00042 #include <calibration_msgs/JointStateCalibrationPattern.h>
00043
00044 using namespace laser_joint_projector;
00045 using namespace std;
00046
00047 void featuresCallback(ros::Publisher* pub,
00048 LaserJointProjector* projector,
00049 const calibration_msgs::JointStateCalibrationPatternConstPtr& msg)
00050 {
00051 sensor_msgs::PointCloud cloud = projector->project(msg->joint_points, ros::Duration());
00052 cloud.header.stamp = msg->header.stamp;
00053 cloud.header.frame_id = "torso_lift_link";
00054 pub->publish(cloud);
00055 }
00056
00057 int main(int argc, char** argv)
00058 {
00059 ros::init(argc, argv, "projector");
00060 ros::NodeHandle n;
00061
00062 KDL::Tree my_tree;
00063 std::string robot_desc_string;
00064 if (!n.getParam("robot_description", robot_desc_string))
00065 ROS_FATAL("Couldn't get a robot_description from the param server");
00066
00067 if (!kdl_parser::treeFromString(robot_desc_string, my_tree))
00068 {
00069 ROS_ERROR("Failed to construct kdl tree");
00070 return false;
00071 }
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081 LaserJointProjector projector;
00082 projector.configure(my_tree, "torso_lift_link", "laser_tilt_link");
00083
00084
00085 ros::Publisher pub = n.advertise<sensor_msgs::PointCloud>("checkerboard_points", 1);
00086
00087
00088 boost::function<void (const calibration_msgs::JointStateCalibrationPatternConstPtr&)> cb
00089 = boost::bind(&featuresCallback, &pub, &projector, _1);
00090
00091 ros::Subscriber sub = n.subscribe(std::string("joint_state_features"), 1, cb);
00092
00093 ros::spin();
00094
00095 return 0;
00096 }