学习cartographer_ros优秀代码

最新推荐文章于 2022-10-28 22:08:22 发布

jack_20

最新推荐文章于 2022-10-28 22:08:22 发布

阅读量374

点赞数 3

分类专栏： c++ cartographer 文章标签： c++

本文链接：https://blog.csdn.net/jack_20/article/details/112387366

版权

c++ 同时被 2 个专栏收录

47 篇文章 3 订阅

订阅专栏

cartographer

1 篇文章 0 订阅

订阅专栏

using指定别名

enum class TrajectoryState { ACTIVE, FINISHED, FROZEN, DELETED };

using TrajectoryState =
    ::cartographer::mapping::PoseGraphInterface::TrajectoryState;

enum class T{} 或 enum class T : int {}

enum Direction {TOP_LEFT, TOP_RIGHT, BOTTOM_LEFT, BOTTOM_RIGHT};
// error!
enum WindowsCorner {TOP_LEFT, TOP_RIGHT, BOTTOM_LEFT, BOTTOM_RIGHT};

enum class Direction {TOP_LEFT, TOP_RIGHT, BOTTOM_LEFT, BOTTOM_RIGHT};
// OK!
enum class WindowsCorner {TOP_LEFT, TOP_RIGHT, BOTTOM_LEFT, BOTTOM_RIGHT};

enum class Color {black, white, red};
auto white = false; // OK!

costexpr看做常量，可传入数组；而const看做只读，实质仍为变量

#include <iostream>
#include <array>
using namespace std;
constexpr int sqr1(int arg){
    return arg*arg;
}
const int sqr2(int arg){
    return arg*arg;
}
int main()
{
    array<int,sqr1(10)> mylist1;//可以，因为sqr1时constexpr函数
    array<int,sqr2(10)> mylist1;//不可以，因为sqr2不是constexpr函数
    return 0;
}

定义模板函数

template <typename MessageType>
::ros::Subscriber SubscribeWithHandler(
    void (Node::*handler)(int, const std::string&,
                          const typename MessageType::ConstPtr&),
    const int trajectory_id, const std::string& topic,
    ::ros::NodeHandle* const node_handle, Node* const node) {
  return node_handle->subscribe<MessageType>(
      topic, kInfiniteSubscriberQueueSize,
      boost::function<void(const typename MessageType::ConstPtr&)>(
          [node, handler, trajectory_id,
           topic](const typename MessageType::ConstPtr& msg) {
            (node->*handler)(trajectory_id, topic, msg);
          }));
}

std::unique_ptr<int> 智能指针，可通过 get() 接口转为普通指针类型
无名命名空间 namespace {}

#include<iostream>
using namespace std;

namespace
{
    int a;
    void f(){cout<<"a";}
    int g() {return 1;}
}

int main()
{
    f();cout<<endl;
    cout<<g()<<endl;
}

/*
a
1
*/

定义无名命名空间后,外部即不能得知无名命名空间的成员名字，即不让外部知道我的成员名字及其调用
由于没有名字，所以其它文件无法引用，它只能在本文件的作用域内有效，
它的作用域:重无名命名空间声明开始到本文件结束。在本文件使用无名命名空间成员时不必用命名空间限定。其实无名命名空间和static是同样的道理，都是只在本文件内有效，无法被其它文件引用。

C++中的双冒号 ::

/* Linux下串口打开、关闭的api */
// fcntl.h文件下的全局函数open 
open (const char *__path, int __oflag, ...)

// unistd.h文件下的全局函数
extern int close (int __fd);
----------------------------------------------------------------------------------
/* 由于每次找api是一件非常浪费coding时间，而且是没多大意义的事情，我现在要将这个函数封装成一个我自己的个人串口库WzSerialPort.h、WzSerialPort.cpp */
// WzSerialPort.h
class WzSerialPort
{
public:
    // ...
    bool open();
    void close();
    // ...
};
---------------------------------------------------------------------------------
/* 注意以下的cpp文件，如果没有 :: 则会报错误，因为WzSerialPort库中有函数open和close，跟全局函数open和close名字相同，如果不做全局与局部的区分，则无法调用到全局函数 */
// WzSerialPort.cpp
bool WzSerialPort::open()
{
    if( ::open(portname,O_RDWR|O_NOCTTY|O_NONBLOCK) != -1 )
    	return true;
    else
	return false;
}
void WzSerialPort::close()
{
    ::close(fd);
}

播放rosbag时，若参数/use_sim_time 为true，
- 则此时ros::WallTime::now()为当前的真实时间，也就是墙上的挂钟时间，一直在走。
- ros::Time::now()为rosbag当时的时间，是由bag中/clock获取的，是仿真时间。
map容器之emplace接口

std::map<int, ::cartographer::mapping::PoseExtrapolator> extrapolators_;
extrapolators_.emplace(
      std::piecewise_construct,
      std::forward_as_tuple(trajectory_id),
      std::forward_as_tuple(::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec),gravity_time_constant));
-----------------------------------------------------------------------------------
map<string, complex<double>> scp;
scp.emplace(piecewise_construct,
            forward_as_tuple("hello"),
            forward_as_tuple(1, 2));
-----------------------------------------------------------------------------------
/*所以对于 map 来说你虽然避免了临时变量的构造，但是你却需要构建两个 tuple 。 这种代价是否值得需要代码编写者自己考虑，从方便性和代码优雅性上来说：scp.insert({"world", {1, 2}});
这种写法都要胜过前面这个 emplace 版本。所以个人认为对于临时变量构建代价不是很大的对象（比如基础类型）推荐使用 insert 而不是 emplace*/
/*注：forward_as_tuple重载了比较运算符== > <等等，依次对tuble元素进行比较*/

ros::master::getTopics()

/*Q: 有时候, 通过launch文件会 include 几个 launch文件, 而其中的几个node之间需要有先后关系, 那么怎么办呢. 
A: 例如: node A 发布topic, service, 然后启用 node B. 那么可以采用 rostopic list 检测 node A是否已经完成了 topic, 然后启动 node B. 用程序实现.*/
    bool waitfor_topic_ok(const char *topic_wait, uint32_t timeout_sec)
    {
        uint32_t iCnt = 0;

        iCnt = 0;
        std::string wait_topic(topic_wait);

        ros::master::V_TopicInfo master_topics;
        while (1) {
            ros::master::getTopics(master_topics);
            ros::master::V_TopicInfo::iterator master_it = master_topics.begin();
            ros::master::V_TopicInfo::iterator master_end = master_topics.end();
            for ( ; master_it != master_end; ++master_it )
            {
                const ros::master::TopicInfo& info = *master_it;
                if ( wait_topic == info.name )
                {
                    return true;
                }
            }
            if (timeout_sec == 0) { //用户检测是否存在
                break; //
            }
            else { //没有发现, 则延迟等待
                sleep(1);
                if ((timeout_sec > 0) && (timeout_sec <= iCnt++)) break;
            }
        }

        return false;
    }

//同理. 其他如 service action 等等.

2. 查看自己的topic是否被订阅.

    //查看 mytopic_pub_ 是否被订阅
    if (mytopic_pub_.getNumSubscribers() == 0){
        //还没有订阅的人
    }

nodehandle之resolvedName()接口

/*resolveName获取绝对路径的话题
::ros::master::V_TopicInfo ros_topics;
::ros::master::getTopics(ros_topics);
for (const auto& it : ros_topics) {
    std::string resolved_topic = node_.resolveName(it.name, false);
    LOG(WARNING) << "topic = " << it.name
                 << "   resolved—topic = " << resolved_topic;
}*/
-----------------------------------------------------------------------------
<node pkg="master" type="Client" name="client">
    <remap from="remote_vel" to="dd"/>
</node>
-----------------------------------------------------------------------------
ros::NodeHandle node;
remote_vel_pub_ = node.advertise<geometry_msgs::Twist>("remote_vel", 1);
std::string resolved_topic = node.resolveName("remote_vel", true);
LOG(WARNING) << "resolved—topic = " << resolved_topic;//输出/dd
/*输出resolved—topic = /dd*/
-----------------------------------------------------------------------------
ros::NodeHandle private_nh("~/agv_info");
remote_switch_pub_ = private_nh.advertise<std_msgs::Int32>("remote_switch", 1);
std::string resolved_topic = private_nh.resolveName("remote_switch", false);
LOG(WARNING) << "resolved—topic = " << resolved_topic;
/*输出resolved—topic = /client/agv_info/remote_switch*/

ros::WallTimer的使用

std::vector<::ros::WallTimer> wall_timers_;

wall_timers_.push_back(node_handle_.createWallTimer(
      ::ros::WallDuration(node_options_.submap_publish_period_sec),
      &Node::PublishSubmapList, this));
wall_timers_.push_back(node_handle_.createWallTimer(
      ::ros::WallDuration(node_options_.trajectory_publish_period_sec),
      &Node::PublishTrajectoryNodeList, this));
wall_timers_.push_back(node_handle_.createWallTimer(
      ::ros::WallDuration(node_options_.trajectory_publish_period_sec),
      &Node::PublishLandmarkPosesList, this));
wall_timers_.push_back(node_handle_.createWallTimer(
      ::ros::WallDuration(kConstraintPublishPeriodSec),
      &Node::PublishConstraintList, this));
/*oneshot参数，true表示延迟后只运行一次，如果已经执行过，还可以通过stop()、setPeriod(ros::Duration)和start()来规划再执行一次。*/
wall_timers_.push_back(node_handle_.createWallTimer(
      ::ros::WallDuration(kTopicMismatchCheckDelaySec),
      &Node::MaybeWarnAboutTopicMismatch, this, /*oneshot=*/true));

absl::StrCat 会计算所需要的字符串长度，预留足够的空间，并把输入的数据级联并存储在输出里

/*可以用absl::StrCat/absl::StrAppend来转化int32_t, uint32_t, int64_t, uint64_t, float, double, const char*, 和string_view*/
const int trajectory_id, const std::set<TrajectoryState>& valid_states) {
status_response.message =
    absl::StrCat("Trajectory ", trajectory_id, " doesn't exist.");
status_response.message =
    absl::StrCat("Trajectory ", trajectory_id, " is in '",
                  TrajectoryStateToString(it->second), "' state.");

jack_20

关注

3
点赞
踩
3

收藏

觉得还不错? 一键收藏
1
评论
学习cartographer_ros优秀代码

using指定别名 enum class TrajectoryState { ACTIVE, FINISHED, FROZEN, DELETED };using TrajectoryState = ::cartographer::mapping::PoseGraphInterface::TrajectoryState;enum class T{} 或 enum class T : int {}enum Direction {TOP_LEFT, TOP_RIGHT, BOTT...
复制链接

扫一扫