octomap(2):参数latch分析与ros advertise函数参数分析

目录

一 参数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 编译测试：

参考链接：

---

 

一 参数latch分析

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订阅者订阅该话题（及其他条件）时，该话题才会发布

1  launch文件的一种配置为：（latch为true）

	<node pkg="octomap_server" type="octomap_server_node" name="octomap_server_ground">
		<param name="resolution" value="0.05" />		
		<param name="frame_id" type="string" value="velo_link" />		
		<param name="sensor_model/max_range" value="25.0" />
		<remap from="cloud_in" to="/points_ground" />	
		<param name="pointcloud_max_z" value="100" />
		<param name="pointcloud_min_z" value="-2.0" />
		<param name="latch" value="true" />
	</node>

2  latch传递值给m_latchedTopics

下载octomap_mapping的源码：https://github.com/OctoMap/octomap_mapping，里面包括octomap_server

用vscode打开octomap_mapping，搜索latch，发现latch的值传递给了m_latchedTopics

3   查询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<visualization_msgs::MarkerArray>("occupied_cells_vis_array", 1, m_latchedTopics);
  m_binaryMapPub = m_nh.advertise<Octomap>("octomap_binary", 1, m_latchedTopics);
  m_fullMapPub = m_nh.advertise<Octomap>("octomap_full", 1, m_latchedTopics);
  m_pointCloudPub = m_nh.advertise<sensor_msgs::PointCloud2>("octomap_point_cloud_centers", 1, m_latchedTopics);
  m_mapPub = m_nh.advertise<nav_msgs::OccupancyGrid>("projected_map", 5, m_latchedTopics);
  m_fmarkerPub = m_nh.advertise<visualization_msgs::MarkerArray>("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);

4 判断是否发布特定话题

（1）publishFreeMarkerArray--空闲三维栅格

--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);
  }

（2）publishMarkerArray--占据三维栅格

--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);
  }

（3）publishPointCloud--点云

--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
        }

（4）publishBinaryMap--二值八叉树地图

--704行：

  if (publishBinaryMap)
    publishBinaryOctoMap(rostime);

（5）publishFullMap--完整版八叉树地图

--707行：

  if (publishFullMap)
    publishFullOctoMap(rostime);

二 ros advertise函数参数分析

         ps:这里利用ros wiki的教程，修改一些语句以测试advertise函数参数的第三个参数

        advertise函数的三个参数依次是：发布的话题名、发布话题缓存队列长度、一个bool参数--未知其作用

1 文件目录

        文件目录如下，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 

<?xml version="1.0"?>
<package format="2">
  <name>beginner_tutorials</name>
  <version>0.0.0</version>
  <description>The publisher_advertiser package</description>
  <maintainer email="meng@todo.todo">meng</maintainer>

  <license>TODO</license>

  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>rospy</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>rospy</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>
  <exec_depend>roscpp</exec_depend>
  <exec_depend>rospy</exec_depend>
  <exec_depend>std_msgs</exec_depend>
  <build_depend>message_generation</build_depend>
  <exec_depend>message_runtime</exec_depend>

  <!-- The export tag contains other, unspecified, tags -->
  <export>
    <!-- Other tools can request additional information be placed here -->

  </export>
</package>

talker.cpp

#include "ros/ros.h"
#include "std_msgs/String.h"

#include <sstream>

int main(int argc, char **argv)
{
  ros::init(argc, argv, "talker");
  ros::NodeHandle n;

  // ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);//修改前
  //修改后，也可以改为false；
  //发布话题后，可以用listen节点查询话题，也可以用rostopic echo查询话题
  ros::Publisher chatter_pub = n.advertise<std_msgs::String>("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;
}

 2 编译测试：

开启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