目录
一 参数latch分析
1 launch文件的一种配置为:(latch为true)
2 latch传递值给m_latchedTopics
3 查询m_latchedTopics
4 判断是否发布特定话题
(1)publishFreeMarkerArray--空闲三维栅格
(2)publishMarkerArray--占据三维栅格
(3)publishPointCloud--点云
(4)publishBinaryMap--二值八叉树地图
(5)publishFullMap--完整版八叉树地图
二 ros advertise函数参数分析
1 文件目录
2 编译测试:
参考链接:
ros wiki上面的介绍为:
~latch (bool, default: True for a static map, false if no initial map is given)
~latch(bool,默认值:对于静态地图为True,如果没有给出初始地图则为false)
- Whether topics are published latched or only once per change. For maximum performance when building a map (with frequent updates), set to false. When set to true, on every map change all topics and visualizations will be created.
- 主题是锁定发布还是每次更改仅发布一次。为了在构建地图(频繁更新)时获得最佳性能,请将其设置为false。当设置为true时,每个地图上的所有更改都将创建所有主题和可视化。
结合下面程序运行分析,我理解如下:
latch为true时,会一直发布话题;latch为false时,只在有ros订阅者订阅该话题(及其他条件)时,该话题才会发布
下载octomap_mapping的源码:https://github.com/OctoMap/octomap_mapping,里面包括octomap_server
用vscode打开octomap_mapping,搜索latch,发现latch的值传递给了m_latchedTopics
打开octomap_mapping_ws/src/octomap_mapping/octomap_server/src/OctomapServer.cpp文件,查找m_latchedTopics在程序中的值传递:
(1)m_latchedTopics初始化--55行左右,默认为true
m_latchedTopics(true),
(2)m_latchedTopics:从launch传递参数--164行:
m_nh_private.param("latch", m_latchedTopics, m_latchedTopics);
(3)根据m_latchedTopics,确定地图话题发布模式--165行:(还没理解)
latch为true时:发布锁定(单个发布需要更长时间,所有主题都已准备好)
latch为false时:发布非锁定(话题仅在需要时准备,仅在地图更改时重新发布)
if (m_latchedTopics){
ROS_INFO("Publishing latched (single publish will take longer, all topics are prepared)");
} else
ROS_INFO("Publishing non-latched (topics are only prepared as needed, will only be re-published on map change");
(4)传递给ros advertise函数(貌似没用,参考“二 ros advertise函数参数分析”)
--170行:
m_markerPub = m_nh.advertise("occupied_cells_vis_array", 1, m_latchedTopics);
m_binaryMapPub = m_nh.advertise("octomap_binary", 1, m_latchedTopics);
m_fullMapPub = m_nh.advertise("octomap_full", 1, m_latchedTopics);
m_pointCloudPub = m_nh.advertise("octomap_point_cloud_centers", 1, m_latchedTopics);
m_mapPub = m_nh.advertise("projected_map", 5, m_latchedTopics);
m_fmarkerPub = m_nh.advertise("free_cells_vis_array", 1, m_latchedTopics);
(5)传递给是否发布特定话题的bool参数
--490行:(这里直接判断是否需要发布话题了)
void OctomapServer::publishProjected2DMap(const ros::Time& rostime) {
m_publish2DMap = (m_latchedTopics || m_mapPub.getNumSubscribers() > 0);
if (m_publish2DMap) {
m_gridmap.header.stamp = rostime;
m_mapPub.publish(m_gridmap);
}
}
--506行:
bool publishFreeMarkerArray = m_publishFreeSpace && (m_latchedTopics || m_fmarkerPub.getNumSubscribers() > 0);
bool publishMarkerArray = (m_latchedTopics || m_markerPub.getNumSubscribers() > 0);
bool publishPointCloud = (m_latchedTopics || m_pointCloudPub.getNumSubscribers() > 0);
bool publishBinaryMap = (m_latchedTopics || m_binaryMapPub.getNumSubscribers() > 0);
bool publishFullMap = (m_latchedTopics || m_fullMapPub.getNumSubscribers() > 0);
--622行:
//create marker for free space:
if (publishFreeMarkerArray){
unsigned idx = it.getDepth();
assert(idx < freeNodesVis.markers.size());
geometry_msgs::Point cubeCenter;
cubeCenter.x = x;
cubeCenter.y = y;
cubeCenter.z = z;
freeNodesVis.markers[idx].points.push_back(cubeCenter);
}
--669行:
// finish FreeMarkerArray:
if (publishFreeMarkerArray){
for (unsigned i= 0; i < freeNodesVis.markers.size(); ++i){
double size = m_octree->getNodeSize(i);
freeNodesVis.markers[i].header.frame_id = m_worldFrameId;
freeNodesVis.markers[i].header.stamp = rostime;
freeNodesVis.markers[i].ns = "map";
freeNodesVis.markers[i].id = i;
freeNodesVis.markers[i].type = visualization_msgs::Marker::CUBE_LIST;
freeNodesVis.markers[i].scale.x = size;
freeNodesVis.markers[i].scale.y = size;
freeNodesVis.markers[i].scale.z = size;
freeNodesVis.markers[i].color = m_colorFree;
if (freeNodesVis.markers[i].points.size() > 0)
freeNodesVis.markers[i].action = visualization_msgs::Marker::ADD;
else
freeNodesVis.markers[i].action = visualization_msgs::Marker::DELETE;
}
m_fmarkerPub.publish(freeNodesVis);
}
--567行
//create marker:
if (publishMarkerArray){
unsigned idx = it.getDepth();
assert(idx < occupiedNodesVis.markers.size());
geometry_msgs::Point cubeCenter;
cubeCenter.x = x;
cubeCenter.y = y;
cubeCenter.z = z;
occupiedNodesVis.markers[idx].points.push_back(cubeCenter);
if (m_useHeightMap){
double minX, minY, minZ, maxX, maxY, maxZ;
m_octree->getMetricMin(minX, minY, minZ);
m_octree->getMetricMax(maxX, maxY, maxZ);
double h = (1.0 - std::min(std::max((cubeCenter.z-minZ)/ (maxZ - minZ), 0.0), 1.0)) *m_colorFactor;
occupiedNodesVis.markers[idx].colors.push_back(heightMapColor(h));
}
#ifdef COLOR_OCTOMAP_SERVER
if (m_useColoredMap) {
std_msgs::ColorRGBA _color; _color.r = (r / 255.); _color.g = (g / 255.); _color.b = (b / 255.); _color.a = 1.0; // TODO/EVALUATE: potentially use occupancy as measure for alpha channel?
occupiedNodesVis.markers[idx].colors.push_back(_color);
}
#endif
}
--642行:
// finish MarkerArray:
if (publishMarkerArray){
for (unsigned i= 0; i < occupiedNodesVis.markers.size(); ++i){
double size = m_octree->getNodeSize(i);
occupiedNodesVis.markers[i].header.frame_id = m_worldFrameId;
occupiedNodesVis.markers[i].header.stamp = rostime;
occupiedNodesVis.markers[i].ns = "map";
occupiedNodesVis.markers[i].id = i;
occupiedNodesVis.markers[i].type = visualization_msgs::Marker::CUBE_LIST;
occupiedNodesVis.markers[i].scale.x = size;
occupiedNodesVis.markers[i].scale.y = size;
occupiedNodesVis.markers[i].scale.z = size;
if (!m_useColoredMap)
occupiedNodesVis.markers[i].color = m_color;
if (occupiedNodesVis.markers[i].points.size() > 0)
occupiedNodesVis.markers[i].action = visualization_msgs::Marker::ADD;
else
occupiedNodesVis.markers[i].action = visualization_msgs::Marker::DELETE;
}
m_markerPub.publish(occupiedNodesVis);
}
--695行:
// finish pointcloud:
if (publishPointCloud){
sensor_msgs::PointCloud2 cloud;
pcl::toROSMsg (pclCloud, cloud);
cloud.header.frame_id = m_worldFrameId;
cloud.header.stamp = rostime;
m_pointCloudPub.publish(cloud);
}
--595行:
// insert into pointcloud:
if (publishPointCloud) {
#ifdef COLOR_OCTOMAP_SERVER
PCLPoint _point = PCLPoint();
_point.x = x; _point.y = y; _point.z = z;
_point.r = r; _point.g = g; _point.b = b;
pclCloud.push_back(_point);
#else
pclCloud.push_back(PCLPoint(x, y, z));
#endif
}
--704行:
if (publishBinaryMap)
publishBinaryOctoMap(rostime);
--707行:
if (publishFullMap)
publishFullOctoMap(rostime);
ps:这里利用ros wiki的教程,修改一些语句以测试advertise函数参数的第三个参数
advertise函数的三个参数依次是:发布的话题名、发布话题缓存队列长度、一个bool参数--未知其作用
文件目录如下,include下文件夹为空
meng@meng:~/ideas/ros_ws$ tree
.
└── src
└── beginner_tutorials
├── CMakeLists.txt
├── include
│ └── publisher_advertiser
├── msg
│ └── Num.msg
├── package.xml
├── src
│ ├── listener.cpp
│ └── talker.cpp
└── srv
└── AddTwoInts.srv
7 directories, 6 files
Num.msg
string first_name
string last_name
uint8 age
uint32 score
AddTwoInts.srv
int64 a
int64 b
---
int64 sum
CMakeLists.txt
cmake_minimum_required(VERSION 2.8.3)
project(beginner_tutorials)
## Find catkin and any catkin packages
find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs genmsg)
## Declare ROS messages and services
add_message_files(FILES Num.msg)
add_service_files(FILES AddTwoInts.srv)
## Generate added messages and services
generate_messages(DEPENDENCIES std_msgs)
## Declare a catkin package
catkin_package()
## Build talker and listener
include_directories(include ${catkin_INCLUDE_DIRS})
add_executable(talker src/talker.cpp)
target_link_libraries(talker ${catkin_LIBRARIES})
add_dependencies(talker beginner_tutorials_generate_messages_cpp)
add_executable(listener src/listener.cpp)
target_link_libraries(listener ${catkin_LIBRARIES})
add_dependencies(listener beginner_tutorials_generate_messages_cpp)
package.xml
beginner_tutorials
0.0.0
The publisher_advertiser package
meng
TODO
catkin
roscpp
rospy
std_msgs
roscpp
rospy
std_msgs
roscpp
rospy
std_msgs
message_generation
message_runtime
talker.cpp
#include "ros/ros.h"
#include "std_msgs/String.h"
#include
int main(int argc, char **argv)
{
ros::init(argc, argv, "talker");
ros::NodeHandle n;
// ros::Publisher chatter_pub = n.advertise("chatter", 1000);//修改前
//修改后,也可以改为false;
//发布话题后,可以用listen节点查询话题,也可以用rostopic echo查询话题
ros::Publisher chatter_pub = n.advertise("chatter", 1000,true);
ros::Rate loop_rate(10);
int count = 0;
//修改后,这样的修改是为了使msg.data保持不变----------
std_msgs::String msg;
std::stringstream ss;
ss << "hello world " << count;
msg.data = ss.str();
//------------------------
while (ros::ok())
{
//修改前
/* std_msgs::String msg;
std::stringstream ss;
ss << "hello world " << count;
msg.data = ss.str(); */
ROS_INFO("%s", msg.data.c_str());
chatter_pub.publish(msg);
ros::spinOnce();
loop_rate.sleep();
// ++count;//meng 修改后
}
return 0;
}
listener.cpp
#include "ros/ros.h"
#include "std_msgs/String.h"
void chatterCallback(const std_msgs::String::ConstPtr& msg)
{
ROS_INFO("I heard: [%s]", msg->data.c_str());
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "listener");
ros::NodeHandle n;
// ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);//修改前
ros::Subscriber sub = n.subscribe("chatter", 1, chatterCallback);//修改后,改了队列长度,但没有用
ros::spin();
return 0;
}
开启roscore
然后:
cd xxx/ros_ws
catkin_make
source devel/setup.bash
rosrun beginner_tutorials talker
新开一个终端:
cd xxx/ros_ws
source devel/setup.bash
rosrun beginner_tutorials listener
可以发现:
无论是将advertise函数的第三个参数改为flase或true,或者将msg.data参数内容保持不变,或者更改订阅者的队列长度,
都不能让talker停止一直发布话题,或阻止listener一直接收话题
octomap_server wiki:octomap_server - ROS Wiki