CMake CPP和ROS笔记

m_zhaoguangbao

已于 2022-08-04 18:41:07 修改

阅读量254

点赞数 1

分类专栏：笔记文章标签： c++

于 2022-07-01 20:06:12 首次发布

本文链接：https://blog.csdn.net/m0_47989004/article/details/125564914

版权

笔记专栏收录该内容

2 篇文章 0 订阅

订阅专栏

CMake

前言：本文仅仅为记录CMake的常用操作

CMake更新

which cmake # /usr/bin/cmake
cmake --version # 3.10
# update: build from source(https://cmake.org/download/)
cd cmake-rc
./bootstrap
make 
sudo make install
source ~/.bashrc
which cmake # /usr/local/bin/cmake

Clion开发ROS工程配置

-DCATKIN_DEVEL_PREFIX:PATH=/home/user_name/catkin_ws/devel

CMake范例

生成动态库
这里以针对DynamixelSDK生成对应的MinGW动态库为例，其文件结构如下：

- include/dynamixel_sdk/*.h
- src/dynamixel_sdk/*.cpp

cmake_minimum_required(VERSION 2.8.3)
project(DynamixelSDKMinGW)

## Compile as C++11
add_compile_options(-std=c++11)
#set(CMAKE_CXX_STANDARD 11)

#find_package()

include_directories(
	include/dynamixel_sdk
)

# 搜索当前目录以及子目录中所有的以.cpp结尾的文件，然后把它们保存到native_srcs变量中
file(GLOB_RECURSE native_srcs ${PROJECT_SOURCE_DIR}/src/dynamixel_sdk/*.cpp)

## Declare a C++ library
add_library(dxl_x64_cpp SHARED ${native_srcs})

动态库的使用

# 存放动态库头文件的目录
target_include_directories(${PROJECT_NAME} PUBLIC externel/DynamixelSDK/c++/include)
# 存放动态库的目录
target_link_directories(${PROJECT_NAME} PUBLIC externel/DynamixelSDK/c++)
# 链接动态库
target_link_libraries(${PROJECT_NAME} PRIVATE dxl_x64_cpp)

若要使得编译出来的可执行程序正确执行，需要让程序找到动态库的位置

CPP

C++杂谈
1.语言背后是实现，从设计实现的角度去理解语言特性及语法。
2.语法的存在是为了理解，相较于人的理解更偏向于机器的理解，语言的解释需要经由编译器，而编译器的规则是人制定的，
所以可以在理解的基础上记忆相关语法。
3.随着程序规模的扩大，并行开发以及局部模块开发是必然的，C++命名空间的存在一方面就是为了这种局部化，
其中匿名命名空间更为体现了这一原则，将变量局部在了一个编译单元内，这使得在没法获得全局意识的前提下，安全可靠的编程，
当然这也便于模块化编程的实现，模块化则必然有利于并行开发、多人合作开发。
4.简单面向对象编程思想的理解，构造事物帮你完成一项工作。
5.为了达到举一反三的效果，C++添加了模板属性。

一、C++内存分配
1.new运算符和operator new()介绍
new：是运算符，不可重载，比如A* a = new A; 对于new来说，有new和::new之分，前者位于std
operator new()：是函数，对于operator new来说，分为全局重载和类重载，全局重载是void* ::operator new(size_t size)
在类中重载形式 void* A::operator new(size_t size)。operator new()完成的操作一般只是分配内存，事实上系统默认的全局::operator new(size_t size)也只是调用malloc分配内存，并且返回一个void*指针，而构造函数的调用是在new运算符中完成的。

2.new和operator new之间的关系

A* a = new A；

这里分为两步：
1.分配内存
2.调用A()构造对象
分配内存这一操作就是由operator new(size_t)来完成的，如果类A重载了operator new，那么将调用A::operator new(size_t )，如果没有重载，就调用::operator new(size_t )，全局new操作符由C++默认提供。
因此前面的两步也就是：
1.调用operator new
2.调用构造函数

（1）new ：不能被重载，其行为总是一致的。它先调用operator new分配内存，然后调用构造函数初始化那段内存。
new 操作符的执行过程：

调用operator new分配内存
调用构造函数生成类对象
返回相应指针

（2）operator new：要实现不同的内存分配行为，应该重载operator new，而不是new。
operator new就像operator + 一样，是可以重载的。如果类中没有重载operator new，那么调用的就是全局的::operator new来完成堆的分配。同理，operator new[]、operator delete、operator delete[]也是可以重载的。

3.如何限制对象只能建立在堆上或者栈上
在C++中，类的对象建立分为两种，一种是静态建立，如A a；另一种是动态建立，如A* ptr=new A；
静态建立一个类对象，是由编译器为对象在栈空间中分配内存，是通过直接移动栈顶指针，挪出适当的空间，然后在这片内存空间上调用构造函数形成一个栈对象。使用这种方法，直接调用类的构造函数。
动态建立类对象，是使用new运算符将对象建立在堆空间中。这个过程分为两步，第一步是执行operator new()函数，在堆空间中搜索合适的内存并进行分配；第二步是调用构造函数构造对象，初始化这片内存空间。这种方法，间接调用类的构造函数。

3.1 只能建立在堆上（设置析构函数为Protected）
类对象只能建立在堆上，就是不能静态建立类对象，即不能直接调用类的构造函数。
容易想到将构造函数设为私有。在构造函数私有之后，无法在类外部调用构造函数来构造类对象，只能使用new运算符来建立对象。然而，前面已经说过，new运算符的执行过程分为两步，C++提供new运算符的重载，其实是只允许重载operator new()函数，而operator()函数用于分配内存，无法提供构造功能。因此，这种方法不可以。
当对象建立在栈上面时，是由编译器分配内存空间的，调用构造函数来构造栈对象。当对象使用完后，编译器会调用析构函数来释放栈对象所占的空间。编译器管理了对象的整个生命周期。如果编译器无法调用类的析构函数，情况会是怎样的呢？比如，类的析构函数是私有的，编译器无法调用析构函数来释放内存。所以，编译器在为类对象分配栈空间时，会先检查类的析构函数的访问性，其实不光是析构函数，只要是非静态的函数，编译器都会进行检查。如果类的析构函数是私有的，则编译器不会在栈空间上为类对象分配内存。
因此，将析构函数设为私有，类对象就无法建立在栈上了。代码如下：

class A  
{  
public:  
    A(){}  
    void destory(){delete this;}  
private:  
    ~A(){}  
};

试着使用A a;来建立对象，编译报错，提示析构函数无法访问。这样就只能使用new操作符来建立对象，构造函数是公有的，可以直接调用。类中必须提供一个destory函数，来进行内存空间的释放。类对象使用完成后，必须调用destory函数。
上述方法的一个缺点就是，无法解决继承问题。如果A作为其它类的基类，则析构函数通常要设为virtual，然后在子类重写，以实现多态。因此析构函数不能设为private。还好C++提供了第三种访问控制，protected。将析构函数设为protected可以有效解决这个问题，类外无法访问protected成员，子类则可以访问。
另一个问题是，类的使用很不方便，使用new建立对象，却使用destory函数释放对象，而不是使用delete。（使用delete会报错，因为delete对象的指针，会调用对象的析构函数，而析构函数类外不可访问）这种使用方式比较怪异。为了统一，可以将构造函数设为protected，然后提供一个public的static函数来完成构造，这样不使用new，而是使用一个函数来构造，使用一个函数来析构。代码如下，类似于单例模式：

class A  
{  
protected:  
    A(){}  
    ~A(){}  
public:  
    static A* create()  
    {  
        return new A();  
    }  
    void destory()  
    {  
        delete this;  
    }  
};

这样，调用create()函数在堆上创建类A对象，调用destory()函数释放内存。

3.2 只能建立在栈上(重载new函数设为私有)
只有使用new运算符，对象才会建立在堆上，因此，只要禁用new运算符就可以实现类对象只能建立在栈上。将operator new()设为私有即可。代码如下：

class A  
{  
private:  
    void* operator new(size_t t){}     // 注意函数的第一个参数和返回值都是固定的  
    void operator delete(void* ptr){}  // 重载了new就需要重载delete  
public:  
    A(){}  
    ~A(){}  
};

ROS

ros doc

example search roscpp wiki
rosorg
click code API and Msg/Srv API
roscpp

ros分布式多机通信

hostname及ifconfig查看当前主机名及ip地址，假设从机名为：zhao-Lenovo-Legion-R7000-2020，ip地址：192.168.51.58；主机名为：super-com，ip地址：192.168.51.184。

设置/etc/hosts和~/.bashrc文件

hosts文件设置
从机hosts文件设置

127.0.0.1       localhost
127.0.1.1       zhao-Lenovo-Legion-R7000-2020  # added
192.168.51.58  	zhao-Lenovo-Legion-R7000-2020  # added
192.168.51.184  super-com					   # added

# The following lines are desirable for IPv6 capable hosts
::1     ip6-localhost ip6-loopback
fe00::0 ip6-localnet
ff00::0 ip6-mcastprefix
ff02::1 ip6-allnodes
ff02::2 ip6-allrouters

主机hosts文件设置

127.0.0.1       localhost
127.0.1.1       super-com  					   # added
192.168.51.184  super-com					   # added
192.168.51.58  	zhao-Lenovo-Legion-R7000-2020  # added

# The following lines are desirable for IPv6 capable hosts
::1     ip6-localhost ip6-loopback
fe00::0 ip6-localnet
ff00::0 ip6-mcastprefix
ff02::1 ip6-allnodes
ff02::2 ip6-allrouters

设置完hosts后，相互ping一下，即主机：ping zhao-Lenovo-Legion-R7000-2020，从机：ping super-com
.bashrc文件设置
bashrc主要设置ROS_HOSTNAME及ROS_MASTER_URI，其中ROS_MASTER_URI指明主机的地址，ROS_HOSTNAME指名自己的地址，另外还存在ROS_IP环境变量和ROS_HOSTNAME作用相同，但需要用IP地址。
从机.bashrc文件

export ROS_HOSTNAME=zhao-Lenovo-Legion-R7000-2020
export ROS_MASTER_URI=http://super-com:11311

主机.bashrc文件

export ROS_HOSTNAME=super-com
export ROS_MASTER_URI=http://super-com:11311

注意：
这里必须按照这种方式设置，设置不正确可能会导致从机上的topic虽然可以通过rostopic list去查找到，但无法通过rostopic echo /topicname查看。

rospy usage

如何在命令行中用rosrun调用py脚本

make sure the python script is executable (chmod +x pythonscript.py).
make sure you have a line at the top of the script that looks like #!/usr/bin/env python (or other path of python interpreter).

get param refer to ros param

// command line
// rosrun jaka_ros_driver connect_robot _robot_ip:="192.168.51.83"
ros::init(argc, argv, "connect_robot");
ros::NodeHandle nh_private("~");

string robot_ip;
nh_private.getParam("robot_ip", robot_ip);

// init params
ros::param::set("/enable_robot", false);
ros::param::set("/disable_robot", false);

在一个功能包中导入另一个功能包的python模块

主要参考：ROS中在一个功能包中导入另一个功能包的python模块

CMakeLists.txt中添加catkin_python_setup()表示需要安装python包，这样catkin在编译时就会在这个功能包的根目录下自动寻找setup.py文件并执行
编辑setup.py
graspit_command

## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD

from setuptools import setup
from catkin_pkg.python_setup import generate_distutils_setup

requirements = [line.strip() for line in open("requirements.txt")]

d = generate_distutils_setup()

d['name'] = "graspit_commander"
d['description'] = "Python interface for GraspIt! simulator"
d['packages'] = ['graspit_commander']
d['package_dir'] = {'': 'src'}
d['requirements'] = requirements

setup(**d)

packages代表你想要安装的python包
package_dir描述的是python包的存放路径，这个必须得写否则你的python包只能放在setup.py的同级目录
setup(**d)其实就是调用distutils包的setup模块
catkin_make实现安装到ros环境
调用时注意原package中的__init__.py文件的导出内容

from graspit_commander import GraspitCommander

在一个功能包中导入另一个功能包生成的库

设生成库的包为A，引用库的包为B
A包设置
其中包A的CMakeList.txt

catkin_package(
  INCLUDE_DIRS include
#  LIBRARIES ros_utils
#  CATKIN_DEPENDS geometry_msgs message_generation message_runtime pcl_conversions pcl_ros roscpp rospy sensor_msgs std_msgs tf
#  DEPENDS system_lib
)

add_library(utils_lib src/utils/utils.cpp)
target_link_libraries(utils_lib ${OpenCV_LIBS} ${PCL_LIBRARIES} ${catkin_LIBRARIES})

这里要注意catkin_package应包含对应的头文件目录，否则在B包中找不到对应的头文件。A包的package.xml

<export>
  <!-- Other tools can request additional information be placed here -->
  <cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -lutils_lib"/>
</export>

B包设置

find_package(catkin REQUIRED COMPONENTS A)
target_link_libraries(node_name ${catkin_LIBRARIES} utils_lib)

<build_depend>A</build_depend>
<build_export_depend>A</build_export_depend>
<exec_depend>A</exec_depend>

roscpp usage

//
// Created by zhao on 2022/6/29.
//

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <tf/LinearMath/Transform.h>
#include <Eigen/Dense>


bool TransformToMatrix(const tf::StampedTransform& transform, Eigen::Matrix4f& transform_matrix)
{
    Eigen::Translation3f tl_btol(
            transform.getOrigin().getX(),
            transform.getOrigin().getY(),
            transform.getOrigin().getZ());
    double roll, pitch, yaw;
    tf::Matrix3x3(transform.getRotation()).getEulerYPR(yaw, pitch, roll);
    Eigen::AngleAxisf rot_x_btol(roll, Eigen::Vector3f::UnitX());
    Eigen::AngleAxisf rot_y_btol(pitch, Eigen::Vector3f::UnitY());
    Eigen::AngleAxisf rot_z_btol(yaw, Eigen::Vector3f::UnitZ());
    transform_matrix = (tl_btol * rot_z_btol * rot_y_btol * rot_x_btol).matrix();
    return true;
}


tf::StampedTransform ListenTransform(const std::string& p_frame, const std::string& c_frame, const std::string& msg){
    tf::TransformListener listener;
    listener.waitForTransform(p_frame, c_frame, ros::Time::now(), ros::Duration(5.0));
    tf::StampedTransform transform;
    listener.lookupTransform(p_frame, c_frame,
                             ros::Time(1), transform);
    std::cout<<msg<<'\n';
    Eigen::Matrix4f transform_matrix;
    TransformToMatrix(transform, transform_matrix);
    std::cout<<transform_matrix<<'\n';
    return transform;
}

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

    ros::Rate rate(1.0);
    while (node.ok()){

        try{
            ListenTransform("tool0", "rg2_eef_link", "tcp_eeT");
            ListenTransform("rg2_eef_link", "tool0", "ee_tcpT");
        }
        catch (tf::TransformException& ex){
            ROS_ERROR("%s",ex.what());
            ros::Duration(1.0).sleep();
        }

        rate.sleep();
    }
    return 0;
};

//
// Created by zhao on 2022/6/29.
//

#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <vector>
#include <fstream>
#include <sstream>

#include <tf2_ros/static_transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>

using namespace std;

//tf2_ros::StaticTransformBroadcaster static_broadcaster;

vector<vector<float> > grasps;
vector<vector<float> > grasps_camera;

geometry_msgs::TransformStamped grasps_tf;
geometry_msgs::TransformStamped grasps_camera_tf;

string grasp_camera_path="/home/zhao/catkin_sandbox_ws/src/multi_fin_grasp/scripts/temp_camera_graspsT.txt";  // camera
string grasp_path="/home/zhao/catkin_sandbox_ws/src/multi_fin_grasp/scripts/temp_grasps.txt";  // Link_0

void read_grasp(const string& path, vector<vector<float> >& gg)
{
    gg.clear();
    ifstream grasp_file (path);
    if (grasp_file.is_open())
    {
        string line;
        while(getline (grasp_file, line))
        {
            vector<float> grasp(7);
            stringstream ss(line);
            for(int i=0; i < 7; i++)
            {
                ss>>grasp[i];
            }
            gg.push_back(grasp);
        }
    }
    else
    {
        std::cerr << "Unable to open file";
    }
}


void ReadTimerCallback(const ros::TimerEvent&){
    read_grasp(grasp_path, grasps);
    read_grasp(grasp_camera_path, grasps_camera);
    std::cout<<"read once"<<'\n';
}

geometry_msgs::TransformStamped CreateTransformStamped(const string& p_frame, const string& c_frame, const vector<float>& pos_quat){
    assert(pos_quat.size() == 7);
    geometry_msgs::TransformStamped static_transformStamped;
    static_transformStamped.header.stamp = ros::Time::now();
    static_transformStamped.header.frame_id = p_frame;
    static_transformStamped.child_frame_id = c_frame;
    static_transformStamped.transform.translation.x = pos_quat.at(0);
    static_transformStamped.transform.translation.y = pos_quat.at(1);
    static_transformStamped.transform.translation.z = pos_quat.at(2);
    static_transformStamped.transform.rotation.x = pos_quat.at(3);
    static_transformStamped.transform.rotation.y = pos_quat.at(4);
    static_transformStamped.transform.rotation.z = pos_quat.at(5);
    static_transformStamped.transform.rotation.w = pos_quat.at(6);
    return static_transformStamped;
}


int main( int argc, char** argv )
{
    ros::init(argc, argv, "grasp_marker");
    ros::NodeHandle n;
    ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("grasp_points", 10);

    tf2_ros::TransformBroadcaster br;

    ros::Rate r(30);
    read_grasp(grasp_path, grasps);
    read_grasp(grasp_camera_path, grasps_camera);

    ros::Timer read_timer = n.createTimer(ros::Duration(5), ReadTimerCallback);


    while (ros::ok())
    {
        visualization_msgs::Marker points, points_camera;
        points_camera.header.frame_id = "/pre_grasp_camera";
        points.header.frame_id = "/base_link";
        points_camera.header.stamp = points.header.stamp = ros::Time::now();
        points_camera.ns = points.ns = "grasp_points";
        points_camera.action = points.action = visualization_msgs::Marker::ADD;
        points.id = 0;
        points_camera.id = 1;
        points.type = visualization_msgs::Marker::POINTS;
        points_camera.type = visualization_msgs::Marker::POINTS;
        points_camera.pose.orientation.w = points.pose.orientation.w = 1.0;

        points_camera.scale.x = points.scale.x = 0.02;
        points_camera.scale.y = points.scale.y = 0.02;

        points.color.g = 1.0f;
        points_camera.color.r = 1.0f;
        points_camera.color.a = points.color.a = 1.0;

//        std::cout<<grasps_camera.size()<<' '<<grasps.size()<<'\n';

        for(vector<float>& grasp : grasps){
            geometry_msgs::Point p;
            p.x = grasp[0];
            p.y = grasp[1];
            p.z = grasp[2];
            points.points.push_back(p);
            grasps_tf = CreateTransformStamped("/base_link", "grasps_tf", grasp);
            break;
        }

        for(vector<float>& grasp : grasps_camera){
            geometry_msgs::Point p;
            p.x = grasp[0];
            p.y = grasp[1];
            p.z = grasp[2];
            points_camera.points.push_back(p);
            grasps_camera_tf = CreateTransformStamped("/pre_grasp_camera", "grasps_camera_tf", grasp);
            break;
        }

//        br.sendTransform(transformStamped);

        marker_pub.publish(points);
        marker_pub.publish(points_camera);
        br.sendTransform(grasps_tf);
        br.sendTransform(grasps_camera_tf);

        r.sleep();
    }
}