37 #include <laser_geometry/laser_geometry.h>
38 #include <sensor_msgs/LaserScan.h>
39 #include <sensor_msgs/PointCloud2.h>
40 #include <tf/transform_listener.h>
41 #include <pcl_ros/transforms.h>
42 #include <pcl/conversions.h>
43 #include <pcl/point_cloud.h>
44 #include <pcl/point_types.h>
45 #include <pcl/segmentation/extract_polygonal_prism_data.h>
46 #include <pcl/filters/extract_indices.h>
47 #include <pcl/filters/project_inliers.h>
48 #include <sensor_msgs/JointState.h>
49 #include <pcl_conversions/pcl_conversions.h>
51 #define PFLN printf("file %s line %d\n",__FILE__,__LINE__);
62 pcl::PointCloud<pcl::PointXYZ> pc_in;
63 pcl::PointCloud<pcl::PointXYZ> pc_transformed;
64 tf::StampedTransform transform;
69 p_listener->lookupTransform(pcmsg_in->header.frame_id,
"/atc/ptu/tilt_block", ros::Time(0), transform);
71 catch (tf::TransformException ex)
73 ROS_ERROR(
"%s",ex.what());
74 ROS_ERROR(
"Cannot find transform, returning");
79 pcl::PCLPointCloud2 pcl_pc;
80 pcl_conversions::toPCL(*pcmsg_in, pcl_pc);
81 pcl::fromPCLPointCloud2(pcl_pc, pc_in);
83 pcl_ros::transformPointCloud(pc_in,pc_transformed,transform.inverse());
84 pc_transformed.header.frame_id =
"/atc/ptu/tilt_block";
86 double target_x = pc_transformed.points[0].x;
87 double target_y = pc_transformed.points[0].y;
88 double target_z = pc_transformed.points[0].z;
90 double delta_pan = atan2(target_y,target_x);
91 double delta_tilt = atan2(target_z, sqrt(target_x*target_x+target_y*target_y));
96 sensor_msgs::JointState joint_state;
97 joint_state.header.stamp = ros::Time::now();
98 joint_state.name.resize(2);
99 joint_state.position.resize(2);
101 joint_state.name[0] =ros::names::remap(
"pan");
102 joint_state.position[0] = new_pan;
104 joint_state.name[1] =ros::names::remap(
"tilt");
105 joint_state.position[1] = new_tilt;
107 pub.publish(joint_state);
114 int main(
int argc,
char **argv)
117 ros::init(argc, argv,
"foveation_control");
118 ros::NodeHandle n(
"~");
119 tf::TransformListener listener(n,ros::Duration(10));
126 pub = n.advertise<sensor_msgs::JointState>(
"/ptu_cmd", 1);
128 ros::Rate loop_rate(10);
int main(int argc, char **argv)
tf::TransformListener * p_listener
void target_pose_cb(const sensor_msgs::PointCloud2ConstPtr &pcmsg_in)