the cloud doesnt move in z axis
hi I'm getting a pcl with a laser, but the cloud does not move in the z axis.
I made a concatenation of data and the broadcaster TF shipping displacement z.
Annex the code of the cloud.
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
double c=0;
void transformada()
{
static tf::TransformBroadcaster br;
c=c+0.01;
tf::Transform transform;
transform.setOrigin( tf::Vector3(0, 0, c) );
transform.setRotation( tf::Quaternion(1, 0, 0, 0) );
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "laser"));
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "my_tf_broadcaster");
ros::NodeHandle node;
while (ros::ok())
{
transformada();
}
ros::spin();
return 0;
}
#include <stdio.h>
#include <fstream>
//ROS
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <laser_geometry/laser_geometry.h>
//PCL_ROS
#include <pcl_ros/point_cloud.h>
#include <pcl_ros/transforms.h>
#include <pcl_ros/impl/transforms.hpp>
//TF
#include <tf/transform_listener.h>
//PCL
#include <pcl/io/pcd_io.h>
ros::Publisher publicador;
laser_geometry::LaserProjection projector_;
pcl::PointCloud<pcl::PointXYZ>::Ptr nubespcl (new pcl::PointCloud<pcl::PointXYZ>);
//pcl::PointCloud<pcl::PointXYZ> nubespcl;
tf::TransformListener *listener_;
void escaneo (const sensor_msgs::LaserScan::ConstPtr& scan_in)
{
sensor_msgs::PointCloud2::Ptr nubemsg(new sensor_msgs::PointCloud2);
//sensor_msgs::PointCloud2 nubemsg;
projector_.transformLaserScanToPointCloud("base_laser",*scan_in,*nubemsg,*listener_);
pcl::PointCloud<pcl::PointXYZ>::Ptr nubepcl (new pcl::PointCloud<pcl::PointXYZ>);
publicador.publish(nubemsg);
pcl::fromROSMsg (*nubemsg,*nubepcl);
*nubespcl +=*nubepcl;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "nube_de_puntos");
ros::NodeHandle n;
std::string toplector = n.resolveName("scan");
uint32_t size = 20;
std::string top_nube_pub = n.resolveName("ptos_nube");
publicador = n.advertise<sensor_msgs::PointCloud2>(top_nube_pub, size);
listener_=new tf::TransformListener();
nubespcl->header.frame_id="/laser";
ros::Subscriber sub = n.subscribe<sensor_msgs::LaserScan>(toplector, size, escaneo);
ROS_INFO("Listo");
ros::spin();
delete listener_;
pcl::io::savePCDFileASCII ("/home/andres/ros_workspace/nube.pcd",*nubespcl);
return 0;
}
Asked by andresiguano on 2012-03-16 12:41:50 UTC
Comments
What do you mean by "The cloud does not move in the Z axis"? Can you give a screenshot, or some more details?
Asked by Mac on 2012-08-27 12:08:28 UTC