|
@@ -13,6 +13,9 @@ static std::string _point_topic = "/points_raw";
|
|
ros::Publisher point_cloud_pub;
|
|
ros::Publisher point_cloud_pub;
|
|
ros::Subscriber points_sub;
|
|
ros::Subscriber points_sub;
|
|
|
|
|
|
|
|
+static double yawrotate = 0;
|
|
|
|
+
|
|
|
|
+
|
|
|
|
|
|
|
|
|
|
static void points_callback(const sensor_msgs::PointCloud2::ConstPtr &input)
|
|
static void points_callback(const sensor_msgs::PointCloud2::ConstPtr &input)
|
|
@@ -24,25 +27,20 @@ static void points_callback(const sensor_msgs::PointCloud2::ConstPtr &input)
|
|
|
|
|
|
pcl::fromROSMsg(*input, point_cloud);
|
|
pcl::fromROSMsg(*input, point_cloud);
|
|
|
|
|
|
- int pointsize = point_cloud.points.size();
|
|
|
|
|
|
+ int pointsize = point_cloud.width;
|
|
int i;
|
|
int i;
|
|
int nskip = 0;
|
|
int nskip = 0;
|
|
|
|
|
|
pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud_filter(
|
|
pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud_filter(
|
|
new pcl::PointCloud<pcl::PointXYZI>());
|
|
new pcl::PointCloud<pcl::PointXYZI>());
|
|
point_cloud_filter->header = point_cloud.header;
|
|
point_cloud_filter->header = point_cloud.header;
|
|
-
|
|
|
|
- point_cloud_filter->width = 0;
|
|
|
|
- point_cloud_filter->height = 1;
|
|
|
|
-
|
|
|
|
- std::cout<<" point width "<<point_cloud.width<<" size: "<<point_cloud.points.size()<<std::endl;
|
|
|
|
for(i=0;i<pointsize;i++)
|
|
for(i=0;i<pointsize;i++)
|
|
{
|
|
{
|
|
double x,y,z;
|
|
double x,y,z;
|
|
- x = point_cloud.points.at(i).x;
|
|
|
|
- y =point_cloud.points.at(i).y;
|
|
|
|
- z =point_cloud.points.at(i).z;
|
|
|
|
-
|
|
|
|
|
|
+ x = point_cloud.at(i).x;
|
|
|
|
+ y = point_cloud.at(i).y;
|
|
|
|
+ z = point_cloud.at(i).z;
|
|
|
|
+ point_cloud.points.at(i);
|
|
if(std::isnan(x) ||(std::isnan(y))||std::isnan(z))
|
|
if(std::isnan(x) ||(std::isnan(y))||std::isnan(z))
|
|
{
|
|
{
|
|
nskip++;
|
|
nskip++;
|
|
@@ -55,6 +53,13 @@ static void points_callback(const sensor_msgs::PointCloud2::ConstPtr &input)
|
|
}
|
|
}
|
|
|
|
|
|
pcl::PointXYZI point;
|
|
pcl::PointXYZI point;
|
|
|
|
+ if(yawrotate != 0)
|
|
|
|
+ {
|
|
|
|
+ double s = x*cos(yawrotate*M_PI/180.0) - y*sin(yawrotate*M_PI/180.0);
|
|
|
|
+ double t = x*sin(yawrotate*M_PI/180.0)+y*cos(yawrotate*M_PI/180.0);
|
|
|
|
+ x = s;
|
|
|
|
+ y= t;
|
|
|
|
+ }
|
|
point.x = x;
|
|
point.x = x;
|
|
point.y = y;
|
|
point.y = y;
|
|
point.z = z;
|
|
point.z = z;
|
|
@@ -64,7 +69,7 @@ static void points_callback(const sensor_msgs::PointCloud2::ConstPtr &input)
|
|
}
|
|
}
|
|
|
|
|
|
std::cout<<" skip nan point count: "<<nskip<<" out point count: "<<point_cloud_filter->width<<std::endl;
|
|
std::cout<<" skip nan point count: "<<nskip<<" out point count: "<<point_cloud_filter->width<<std::endl;
|
|
- point_cloud_pub.publish(point_cloud_filter);
|
|
|
|
|
|
+ point_cloud_pub.publish(point_cloud);
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
@@ -78,8 +83,10 @@ int main(int argc, char *argv[])
|
|
|
|
|
|
private_nh.getParam("points_topic",_point_topic);
|
|
private_nh.getParam("points_topic",_point_topic);
|
|
private_nh.getParam("sensor_topic",_point_sensor_topic);
|
|
private_nh.getParam("sensor_topic",_point_sensor_topic);
|
|
|
|
+ private_nh.getParam("rotate",yawrotate);
|
|
std::cout<<"_point_topic is "<<_point_topic<<std::endl;
|
|
std::cout<<"_point_topic is "<<_point_topic<<std::endl;
|
|
std::cout<<"_sensor_topic : "<<_point_sensor_topic<<std::endl;
|
|
std::cout<<"_sensor_topic : "<<_point_sensor_topic<<std::endl;
|
|
|
|
+ std::cout<<" yaw rotate: "<<yawrotate<<std::endl;
|
|
|
|
|
|
point_cloud_pub = private_nh.advertise<sensor_msgs::PointCloud2>(
|
|
point_cloud_pub = private_nh.advertise<sensor_msgs::PointCloud2>(
|
|
_point_topic, 10);
|
|
_point_topic, 10);
|