//***给自己的话***//:tare接受的点云数据id_dense是要设置成false的,不然报错；另外雷达intensity是人为设置成1了,还没尝试过不设置会怎么样,但是如果做现实的数据接口记得做好雷达intensify数据.
目标:采用2D雷达替换3D雷达,为Tare_planner提供数据
一.伤口断裂处
1.注释掉tare当中不必要的话题订阅(在sensor_coverage_planner_ground.cpp)
把kCheckTerrainCollision在yaml里默认改为false(取消地势分析)
(这里我把地势分析也注释了,这个不知道不去除的话效果会不会更好;不过2D雷达确实捕捉不到3D地势的特征,所以地势分析似乎是没法提取出有用的地势信息的)
// exploration_start_sub_ =
// nh.subscribe(pp_.sub_start_exploration_topic_, 5, &SensorCoveragePlanner3D::ExplorationStartCallback, this); //这个订阅未被使用
registered_scan_sub_ =
nh.subscribe(pp_.sub_registered_scan_topic_, 5, &SensorCoveragePlanner3D::RegisteredScanCallback, this);//******三D相机********/registered_scan
//EXPL 1.每帧执行pd_.planning_env_的一个操作 => :上传一次机器人位置和pd_.registered_cloud_->cloud_ 2. 每5帧上传kepose ==>> 当前机器人位置"keypose_.pose.pose.position；pd_.keypose_.pose.covariance；pd_.cur_keypose_node_ind_" 和 "pd_.keypose_cloud_->cloud_",
// terrain_map_sub_ = nh.subscribe(pp_.sub_terrain_map_topic_, 5, &SensorCoveragePlanner3D::TerrainMapCallback, this);//局部地势/terrain_map
// //EXPL 1.将接收到的点,intensity达到阈值的都赋值给pd_.terrain_collision_cloud_->cloud_->points (只有这件事情) (这个是局部地势)
// terrain_map_ext_sub_ =
// nh.subscribe(pp_.sub_terrain_map_ext_topic_, 5, &SensorCoveragePlanner3D::TerrainMapExtCallback, this);//更大的地势/terrain_map_ext
// //EXPL 1.将接收到的点,intensity达到阈值的都赋值给pd_.terrain_ext_collision_cloud_ (只有这件事情) (这个是较为全局的地势)
state_estimation_sub_ =
nh.subscribe(pp_.sub_state_estimation_topic_, 5, &SensorCoveragePlanner3D::StateEstimationCallback, this);//位置估计/state_estimation_at_scan
//EXPL 1.跟机器人的位置有关,将位置信息赋值给pd_.initial_position_,(只初始化一次) 2.每次更新yaw(翻译是偏离航线) 3.每次更新机器人位置
//MYTODO 地势的都是在 SensorCoveragePlanner3D::UpdateViewPoints() 这个函数里使用；；如果加入2D想要放弃地势变化的话看看怎么改能让这个函数正常工作
// coverage_boundary_sub_ = //这3个订阅未被使用
// nh.subscribe(pp_.sub_coverage_boundary_topic_, 1, &SensorCoveragePlanner3D::CoverageBoundaryCallback, this);
// viewpoint_boundary_sub_ =
// nh.subscribe(pp_.sub_viewpoint_boundary_topic_, 1, &SensorCoveragePlanner3D::ViewPointBoundaryCallback, this);
// nogo_boundary_sub_ =
// nh.subscribe(pp_.sub_nogo_boundary_topic_, 1, &SensorCoveragePlanner3D::NogoBoundaryCallback, this);2.在vehicle_simulator这里修改了lidar.urdf.xacro,新建了一个文件lidar_2D_urdf.xacro.
这里将两行注释掉,并且新写了它下面的两行(即5.6行),其发布的话题为"/2D_Lascan"(输入)
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="example">
<link name="lidar" />
<!-- <xacro:include filename="$(find velodyne_description)/urdf/VLP-16.urdf.xacro"/>
<xacro:VLP-16 parent="lidar" name="velodyne" topic="/velodyne_points" hz="5" samples="350" min_range="0.1" organize_cloud="true"> -->
<xacro:include filename="$(find velodyne_description)/urdf/lidar_2D_urdf.xacro"/>
<xacro:VLP-16 parent="lidar" name="2D_Ladar" topic="/2D_Lascan" hz="5" samples="350" min_range="0.1" organize_cloud="true">
<origin xyz="0 0 0" rpy="0 0 0" />
</xacro:VLP-16>
</robot>3.由于urdf的修改,这里不再能接受到"/velodyne_points"的数据,新实例化一个订阅对象subScan_new,新写一条回调函数.
ros::Subscriber subScan_new = nh.subscribe<sensor_msgs::LaserScan>("/2D_Lascan", 100, scanHandler_for_2DLadar); //## 接收消息2D并发布到/registered_scan在vehicle_simulator这里修改了vehicleSimulator.cpp,将pubScan的发布话题改成了"/registered_scan_old",即对其进行了废弃.新实例化一个发布对象pubScan_new,发布的话题名称为"/registered_scan"(输出).
二.尝试把功能都能正常做起来,并连接伤口
1.写urdf让gazebo能够正常发出激光雷达数据,这是新建的lidar_2D_urdf.xacro文件
(注释的为VLP-16.urdf.xacro源代码,注释下面是新加的代码=>采用2D雷达reference)
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="VLP-16">
<xacro:property name="M_PI" value="3.1415926535897931" />
<xacro:macro name="VLP-16" params="*origin parent:=base_link name:=velodyne topic:=/velodyne_points organize_cloud:=false hz:=10 lasers:=16 samples:=1875 collision_range:=0.3 min_range:=0.9 max_range:=130.0 noise:=0.008 min_angle:=-${M_PI} max_angle:=${M_PI} gpu:=false">
<joint name="${name}_base_mount_joint" type="fixed">
<xacro:insert_block name="origin" />
<parent link="${parent}"/>
<child link="${name}_base_link"/>
</joint>
<link name="${name}_base_link">
<inertial>
<mass value="0.83"/>
<origin xyz="0 0 0.03585"/>
<inertia ixx="${(0.83 * (3.0*0.0516*0.0516 + 0.0717*0.0717)) / 12.0}" ixy="0" ixz="0"
iyy="${(0.83 * (3.0*0.0516*0.0516 + 0.0717*0.0717)) / 12.0}" iyz="0"
izz="${0.5 * 0.83 * (0.0516*0.0516)}"/>
</inertial>
<visual>
<geometry>
<mesh filename="package://velodyne_description/meshes/VLP16_base_1.dae" />
</geometry>
</visual>
<visual>
<geometry>
<mesh filename="package://velodyne_description/meshes/VLP16_base_2.dae" />
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.03585"/>
<geometry>
<cylinder radius="0.0516" length="0.0717"/>
</geometry>
</collision>
</link>
<joint name="${name}_base_scan_joint" type="fixed" >
<origin xyz="0 0 0.0377" rpy="0 0 0" />
<parent link="${name}_base_link" />
<child link="${name}"/>
</joint>
<link name="${name}">
<inertial>
<mass value="0.01"/>
<origin xyz="0 0 0"/>
<inertia ixx="1e-7" ixy="0" ixz="0" iyy="1e-7" iyz="0" izz="1e-7"/>
</inertial>
<visual>
<origin xyz="0 0 -0.0377" />
<geometry>
<mesh filename="package://velodyne_description/meshes/VLP16_scan.dae" />
</geometry>
</visual>
</link>
<!-- Gazebo requires the velodyne_gazebo_plugins package -->
<!-- <gazebo reference="${name}">
<xacro:if value="${gpu}">
<sensor type="gpu_ray" name="${name}-VLP16">
<pose>0 0 0 0 0 0</pose>
<visualize>false</visualize>
<update_rate>${hz}</update_rate>
<ray>
<scan>
<horizontal>
<samples>${samples}</samples>
<resolution>1</resolution>
<min_angle>${min_angle}</min_angle>
<max_angle>${max_angle}</max_angle>
</horizontal>
<vertical>
<samples>${lasers}</samples>
<resolution>1</resolution>
<min_angle>-${15.0*M_PI/180.0}</min_angle>
<max_angle> ${15.0*M_PI/180.0}</max_angle>
</vertical>
</scan>
<range>
<min>${collision_range}</min>
<max>${max_range+1}</max>
<resolution>0.001</resolution>
</range>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.0</stddev>
</noise>
</ray>
<plugin name="gazebo_ros_laser_controller" filename="libgazebo_ros_velodyne_gpu_laser.so">
<topicName>${topic}</topicName>
<frameName>${name}</frameName>
<organize_cloud>${organize_cloud}</organize_cloud>
<min_range>${min_range}</min_range>
<max_range>${max_range}</max_range>
<gaussianNoise>${noise}</gaussianNoise>
</plugin>
</sensor>
</xacro:if>
<xacro:unless value="${gpu}">
<sensor type="ray" name="${name}-VLP16">
<pose>0 0 0 0 0 0</pose>
<visualize>false</visualize>
<update_rate>${hz}</update_rate>
<ray>
<scan>
<horizontal>
<samples>${samples}</samples>
<resolution>1</resolution>
<min_angle>${min_angle}</min_angle>
<max_angle>${max_angle}</max_angle>
</horizontal>
<vertical>
<samples>${lasers}</samples>
<resolution>1</resolution>
<min_angle>-${15.0*M_PI/180.0}</min_angle>
<max_angle> ${15.0*M_PI/180.0}</max_angle>
</vertical>
</scan>
<range>
<min>${collision_range}</min>
<max>${max_range+1}</max>
<resolution>0.001</resolution>
</range>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.0</stddev>
</noise>
</ray>
<plugin name="gazebo_ros_laser_controller" filename="libgazebo_ros_velodyne_laser.so">
<topicName>${topic}</topicName>
<frameName>${name}</frameName>
<organize_cloud>${organize_cloud}</organize_cloud>
<min_range>${min_range}</min_range>
<max_range>${max_range}</max_range>
<gaussianNoise>${noise}</gaussianNoise>
</plugin>
</sensor>
</xacro:unless>
</gazebo> -->
<!-- 配置雷达传感器信息 -->
<gazebo reference="${name}">
<sensor type="ray" name="rplidar">
<pose>0 0 0 0 0 0</pose>
<visualize>false</visualize>
<update_rate>${hz}</update_rate>
<ray>
<scan>
<horizontal>
<samples>360</samples>
<resolution>1</resolution>
<min_angle>-3</min_angle>
<max_angle>3</max_angle>
</horizontal>
</scan>
<range>
<min>0.10</min>
<max>40.0</max>
<resolution>0.01</resolution>
</range>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.01</stddev>
</noise>
</ray>
<plugin name="gazebo_rplidar" filename="libgazebo_ros_laser.so">
<topicName>${topic}</topicName>
<frameName>${name}</frameName>
</plugin>
</sensor>
</gazebo>
</xacro:macro>
</robot>
此时运行roslaunch vehicle_simulator system_tunnel.launch,再另起一个rviz,应该可以看到
2D雷达的数据可以显示出来了
其中Fixed Frame应该选择2D_Ladar,topic应该选2D_Lascan,这个以lidar.urdf.xacro写的这个为准.
<xacro:VLP-16 parent="lidar" name="2D_Ladar" topic="/2D_Lascan" hz="5" samples="350" min_range="0.1" organize_cloud="true">2.订阅雷达数据,将其转换成pcl2类型
这个如伤口断裂处所言,在visualizationTools.cpp做订阅和发布接口=>在订阅的回调函数中完成数据的转换.
订阅:
ros::Subscriber subScan_new = nh.subscribe<sensor_msgs::LaserScan>("/2D_Lascan", 100, scanHandler_for_2DLadar);回调函数:
(回调函数由两部分组成,一部分从雷达数据laserscan转成pcl2,此时获得的是雷达坐标系下的(x,y)坐标；另一部分由原本scanHandler()函数中的代码构成,用于数据旋转平移到map坐标系下)
void scanHandler_for_2DLadar(const sensor_msgs::LaserScan::ConstPtr &input)
//EXPL 这个承载行参的变量是指针还是对象是有讲究的,这个具体看订阅的时候的书写格式!
{
// cloud.points.clear();
cloud.clear();
copy_cloud.clear();
std::vector<float> ranges = input->ranges;
cloud.points.resize(ranges.size());
//?? 这里给pcl2声明容量
cloud.width = ranges.size();
//?? 这里给pcl2 width,heighy赋值
cloud.height = 1;
cloud.header.stamp = input->header.stamp.toSec();
//?? 这里给pcl2时间戳赋值
cloud.header.frame_id = "map";
//?? 这里给pcl2设置发送ID
/*--------------这段进行"角度range"到(x,y)坐标的转换---------------*/
//转换到二维XY平面坐标系下;
// std::cout<<"cloud其他初始化"<<std::endl;
for(int i=0; i< ranges.size(); i++)
{
double angle = input->angle_min + i * input->angle_increment;
double lX = ranges[i] * cos(angle);
double lY = ranges[i] * sin(angle);
float intensity = input->intensities[i];
cloud.points[i].x=lX;
cloud.points[i].y=lY;
cloud.points[i].z=0;
cloud.points[i].intensity=1;
//BUG 这个intensity注意 => 之后做显示如果能返回正确的intensity的话一定要使用到这个信息,不能直接赋值1
}
/*--------------这段进行"角度range"到(x,y)坐标的转换---------------*/
/*------------------------这段给点云复制,多增加立体感end--------------------------##*/
std::vector<int> mapping;
pcl::removeNaNFromPointCloud(cloud, cloud, mapping);
//TAG 这里去除ranges里面的inf/NaN值不知道有没有用
pcl::copyPointCloud(cloud,copy_cloud);
for(int h=0;h<=20;h+=2)
{
for(int i=0;i<copy_cloud.points.size();i++)
{
copy_cloud.points[i].z=(float)h/10;
}
// std::cout<<"cloud_add.points[i].z: "<<(float)h/10<<std::endl;
cloud+=copy_cloud;
}
/*------------------------这段给点云复制,多增加立体感end----------------------------##*/
/*------------------------------------------------------------上面这段是2D雷达 => 本地(x,y)坐标系---------------------------------------------------------------------------------*/
/*----------------------------------------------------下面这段是本地(x,y)坐标系 => 全局(x,y)坐标系---------------------------------------------------------------------------------*/
if (!systemInited) {
systemInitCount++;
if (systemInitCount > systemDelay) {
systemInited = true;
}
return;
}
double scanTime = cloud.header.stamp;
if (odomSendIDPointer < 0)
{
return;
}
while (odomTimeStack[(odomRecIDPointer + 1) % stackNum] < scanTime &&
odomRecIDPointer != (odomSendIDPointer + 1) % stackNum)
{
odomRecIDPointer = (odomRecIDPointer + 1) % stackNum;
}
double odomRecTime = odomTime.toSec();
float vehicleRecX = vehicleX;
float vehicleRecY = vehicleY;
float vehicleRecZ = vehicleZ;
float vehicleRecRoll = vehicleRoll;
float vehicleRecPitch = vehiclePitch;
float vehicleRecYaw = vehicleYaw;
float terrainRecRoll = terrainRoll;
float terrainRecPitch = terrainPitch;
if (use_gazebo_time)
{
odomRecTime = odomTimeStack[odomRecIDPointer];
vehicleRecX = vehicleXStack[odomRecIDPointer];
vehicleRecY = vehicleYStack[odomRecIDPointer];
vehicleRecZ = vehicleZStack[odomRecIDPointer];
vehicleRecRoll = vehicleRollStack[odomRecIDPointer];
vehicleRecPitch = vehiclePitchStack[odomRecIDPointer];
vehicleRecYaw = vehicleYawStack[odomRecIDPointer];
terrainRecRoll = terrainRollStack[odomRecIDPointer];
terrainRecPitch = terrainPitchStack[odomRecIDPointer];
}
float sinTerrainRecRoll = sin(terrainRecRoll);
float cosTerrainRecRoll = cos(terrainRecRoll);
float sinTerrainRecPitch = sin(terrainRecPitch);
float cosTerrainRecPitch = cos(terrainRecPitch);
scanData->clear();
// pcl::fromROSMsg(*scanIn, *scanData);
pcl::copyPointCloud(cloud,*scanData);
// ?? ## 1获取数据
pcl::removeNaNFromPointCloud(*scanData, *scanData, scanInd);
int scanDataSize = scanData->points.size();
for (int i = 0; i < scanDataSize; i++)
{
float pointX1 = scanData->points[i].x;
float pointY1 = scanData->points[i].y * cosTerrainRecRoll - scanData->points[i].z * sinTerrainRecRoll;
float pointZ1 = scanData->points[i].y * sinTerrainRecRoll + scanData->points[i].z * cosTerrainRecRoll;
float pointX2 = pointX1 * cosTerrainRecPitch + pointZ1 * sinTerrainRecPitch;
float pointY2 = pointY1;
float pointZ2 = -pointX1 * sinTerrainRecPitch + pointZ1 * cosTerrainRecPitch;
float pointX3 = pointX2 + vehicleRecX;
float pointY3 = pointY2 + vehicleRecY;
float pointZ3 = pointZ2 + vehicleRecZ;
scanData->points[i].x = pointX3;
scanData->points[i].y = pointY3;
scanData->points[i].z = pointZ3;
}
// ?? ## 2旋转平移处理
// publish 5Hz registered scan messages
scanData->is_dense=false;
sensor_msgs::PointCloud2 scanData2;
pcl::toROSMsg(*scanData, scanData2); //1.数据 2.时间戳
scanData2.header.stamp = ros::Time().fromSec(odomRecTime);
scanData2.header.frame_id = "map";
pubScanPointer_new->publish(scanData2);
// ?? ## 3发布
}发布pcl2:
scanData->is_dense=false;
sensor_msgs::PointCloud2 scanData2;
pcl::toROSMsg(*scanData, scanData2); //1.数据 2.时间戳
scanData2.header.stamp = ros::Time().fromSec(odomRecTime);
scanData2.header.frame_id = "map";
pubScanPointer_new->publish(scanData2);三.数据流动过程(效果)
gazebo(urdf) => 发布话题frame"2D_Ladar",topic"/2D_Lascan" => 节点vehicleSimulator(也就是上面那个.cpp)订阅 => 回调函数scanHandler_for_2DLadar => 回调函数中laserscan->pcl2 => 回调函数中发布到/registered_scan
图1.urdf发布的2D数据laserscan
图2.节点vehicleSimulator上laserscan转pcl2
图3.节点上z轴方向上复制堆叠并发布到/registered_scan
1802
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
