# orb_slam2学习 **Repository Path**: pigzz2/orb-slam2-learning ## Basic Information - **Project Name**: orb_slam2学习 - **Description**: orb_slam2 学习笔记 - **Primary Language**: C++ - **License**: Apache-2.0 - **Default Branch**: master - **Homepage**: None - **GVP Project**: No ## Statistics - **Stars**: 0 - **Forks**: 0 - **Created**: 2022-10-29 - **Last Updated**: 2022-10-29 ## Categories & Tags **Categories**: Uncategorized **Tags**: None ## README - [创建ROS功能包](#创建ros功能包) - [编写功能包代码](#编写功能包代码) - [velocity_publisher 发布速度话题](#velocity_publisher-发布速度话题) - [pose_subscriber 订阅位置话题](#pose_subscriber-订阅位置话题) - [编译和使用功能包源码](#编译和使用功能包源码) - [自定义消息话题](#自定义消息话题) - [编写自定义消息话题(Person.msg)](#编写自定义消息话题personmsg) - [调用自定义消息话题](#调用自定义消息话题) - [ROS中的C/S实现](#ros中的cs实现) - [Client客户端实现](#client客户端实现) - [Server服务端实现](#server服务端实现) - [服务数据的定义与使用](#服务数据的定义与使用) - [编写自定义服务数据(Person.srv)](#编写自定义服务数据personsrv) - [调用自定义服务数据](#调用自定义服务数据) - [参数的使用和与编程方法](#参数的使用和与编程方法) # 创建ROS功能包 所有功能包**cpp源码**都在工作空间(catkin_ws)下的**src文件夹**中,**python源码**都在**scripts文件夹**中 **how to do** ```sh cd ~/catkin_ws/src # 创建功能包函数 + 功能包名 + 依赖项 catkin_create_pkg learning_topic roscpp rospy std_msgs geometry_msgs turtlesim ``` --- ## 编写功能包代码 ### velocity_publisher 发布速度话题 **how to do:** - 初始化ROS节点； - 向ROS Master注册节点和信息，包括发布的话题名和话题中的消息类型； - 创建消息数据； - 按照一定频率循环发布消息。 **cpp实现：** ```cpp /** * 发布turtle1/cmd_vel话题，消息类型为geometry_msgs::Twist */ #include #include int main(int args, char **argv) { // ROS节点初始化，设置节点名字 ros::init(args, argv, "velocity_publisher"); // 创建节点句柄 ros::NodeHandle n; // 创建一个Publisher，发布名为/turtle1/cmd_vel的topic，消息类型为geometry_msgs::Twist,队列长度10 ros::Publisher turtle_vel_pub = n.advertise("/turtle1/cmd_vel", 10); // 设置循环的频率 ros::Rate loop_rate(10); int count = 0; while (ros::ok()) { // 初始化geometry_msgs::Twitst类型的消息 geometry_msgs::Twist vel_msg; vel_msg.linear.x = 0.5; vel_msg.angular.z = 0.2; // 发布消息 turtle_vel_pub.publish(vel_msg); ROS_INFO("Publish turtle velocity command[%0.2f m/s, %0.2f rad/s]", vel_msg.linear.x, vel_msg.angular.z); // 按照循环频率延时 loop_rate.sleep(); } return 0; } ``` **python实现：** ```python #!/usr/bin/env python # coding:utf-8 import rospy from geometry_msgs.msg import Twist def velocity_publisher(): # ROS节点初始化 rospy.init_node('velocity_publisher', anonymous=True) # 创建一个Publisher，发布/turtle1/cmd_vel的topic，消息类型为geometry_msgs::Twist,队列长度10 turtle_vel_pub = rospy.Publisher('/turtule1/cmd_vel', Twist, queue_size=10) # 设置循环的频率 rate = rospy.Rate(10) while not rospy.is_shutdown(): # 初始化geometry_msgs::Twist类型的消息 vel_msg = Twist() vel_msg.linear.x = 0.5 vel_msg.angular.z = 0.2 # 发布消息 turtle_vel_pub.publish(vel_msg) rospy.loginfo( 'Publish turtule velocity command[%0.2f m/s, %0.2f rad/s]', vel_msg.linear.x, vel_msg.angular.z) # 按照循环频率延时 rate.sleep() if __name__ == '__main__': try: velocity_publisher() except rospy.ROSInterruptException: pass ``` ### pose_subscriber 订阅位置话题 **how to do:** - 初始化ROS节点； - 订阅需要的话题，注册收到数据时的回调函数； - 循环等待话题消息，接受到消息后进入回调函数； - 在回调函数中完成消息处理。**(回调函数无法嵌套，需要高效处理)** cpp实现： ```cpp /** * 订阅/turtle1/pose话题，消息类型为turtlesim::Pose */ #include "turtlesim/Pose.h" #include // 接受到订阅的消息后，会进入消息回调函数 void poseCallback(const turtlesim::Pose::ConstPtr &msg) { // 打印收到的消息 ROS_INFO("Turtle pose: x:%0.6f, y:%0.6f", msg->x, msg->y); } int main(int argc, char **argv) { // 初始化ROS节点 ros::init(argc, argv, "pose_subscriber"); // 创建节点句柄 ros::NodeHandle n; // 创建一个Subscriber，订阅名为/turtle1/pose的topic，注册回调函数poseCallback ros::Subscriber pose_sub = n.subscribe("/turtle1/pose", 10, poseCallback); // 循环等待回调函数 ros::spin(); return 0; } ``` **python实现：** ```python #!/usr/bin/env python # coding:utf-8 import rospy from turtlesim.msg import Pose def poseCallback(msg): rospy.loginfo('Turtle pose: [x: %0.6f, y: %0.6f]', msg.x, msg.y) def pose_subscriber(): # 初始化ROS节点 rospy.init_node('pose_subscriber', anonymous=True) # 订阅需要的话题 pose_subscriber = rospy.Subscriber('/turtle1/pose', Pose, poseCallback) # 循环等待话题消息 rospy.spin() if __name__ == '__main__': pose_subscriber() ``` --- ## 编译和使用功能包源码 **how to do:** - cpp：配置src/CMakelists.txt文件(cpp) ```cmake ########### ## Build ## ########### 在build段落中加入： # 将src/velocity_publisher.cpp编译成可执行文件velocity_publisher.exe add_executable(velocity_publisher src/velocity_publisher.cpp) # 将可执行文件velocity_publisher.exe和ROS接口作链接 target_link_libraries(velocity_publisher ${catkin_LIBRARIES}) ``` ```sh # 在工作空间根目录下 cd ~/catkin_ws # 编译 catkin_make ............. # 更新环境变量（如果在.bashrc中配置了可以跳过） source devel/setup.bash # 启动master roscore # 新建終端，启动海龟模拟器节点 rosrun turtlesim turtlesim_node # 新建終端，启动velocity_publisher功能包，持续发布速度指令 # rosrun + 功能包名 + 功能包编译的可执行文件（同上文CMakeLists.txt中 add_executable()第二个参数相同） rosrun learning_topic velocity_publisher ``` - python： ```sh # 进入python源码根目录 cd ~/learning_toptic/scripts # 给python文件附加可执行权限 sudo chmod +x velocity_publisher.py # 启动master roscore # 新建終端，启动海龟模拟器节点 rosrun turtlesim turtlesim_node # 新建終端，启动velocity_publisher功能包，持续发布速度指令 # rosrun + 功能包名 + python文件 rosrun learning_topic velocity_publisher tips: 如果rosrun + py文件报错 1. 检查是否给py文件加入可执行权限 2. 在py文件头部加入： #!/usr/bin/env python # coding:utf-8 ``` ## 自定义消息话题 ### 编写自定义消息话题(Person.msg) **how to do:** - 定义.msg文件,文件放置于功能包根目录的msg文件夹中（msg/Person.msg） ``` string name uint8 gender uint8 age uint8 unkown = 0 uint8 male = 1 uint8 female = 2 ``` - 在功能包中package.xml中添加功能包依赖 ```xml message_generation message_runtime ``` - 在CMakelists.txt中添加编译选项 ```cmake ################################################ ## Declare ROS messages, services and actions ## ################################################ #和自定义消息文件连接 add_message_files(FILES Person.msg) generate_messages(DEPENDENCIES std_msgs) ################################### ## catkin specific configuration ## ################################### catkin_package( # INCLUDE_DIRS include # LIBRARIES learning_topic CATKIN_DEPENDS geometry_msgs roscpp rospy std_msgs turtlesim message_runtime # DEPENDS system_lib ) ``` - 在工作空间中重新编译(编译之后，devel/include文件夹下会生成自定义msg文件对应头文件) ```sh cd ~/catkin_ws catkin_make ``` ### 调用自定义消息话题 publisher和subscriber的实现和上文流程相同，唯一区别处在于消息话题的名称是自定义的 **("/person_info")**，只需两者对应即可实现消息话题的发布/订阅。 publisher: ```cpp ros::Publisher person_info_pub = n.advertise("/person_info", 10); ``` subscriber: ```cpp ros::Subscriber person_info_sub = n.subscribe("/person_info", 10, Callback); ``` 编写完功能包代码后，流程和上文相同： cpp： 1. 在CMakelists.txt中配置bulid属性 2. 在工作空间下使用catkin_make重新编译 python： 给源码文件附加可执行权限 运行功能包： ```sh # 开启rosmaster roscore # 发布数据 rosrun learning_topic person_publisher # 订阅数据 rosrun learning_topic person_subscriber ``` --- # ROS中的C/S实现 ## Client客户端实现 ```ditaa {cmd=true args=["-E"}} +-----------------+ + + + ROS Master{s}+ +-----------------+ ^ | | +-----------------------------------+--------------------------------------+ |Registration Registration| | | | | v v +---------------+ Request +-------------+ Request +---------------+ + Client + ------------------> + Service + ------------------> + Server + + + turtlesim.Spawn + {io} + turtlesim.Spawn + + +(turtle_spawn) + <------------------ + (/spawn) + <------------------ + (turtlesim) + +---------------+ Response +-------------+ Response +---------------+ ``` **how to do:** - 初始化ROS节点； - 创建一个Client实例； - 发布服务器请求数据； - 等待Server处理之后的应答结果。 ```sh # 进入工作空间根目录 cd ~/catkin_ws/srs # 创建learning_service功能包 catkin_create_pkg learning_service roscpp rospy std_msgs geometry_msgs turtlesim ``` **cpp实现：** ```cpp /* 请求/spawn服务，服务数据类型turtlesim::Spawn */ #include #include int main(int argc, char **argv) { ros::init(argc, argv, "turtle_spawn"); ros::NodeHandle n; // 发现/spawn服务后，创建一个服务客户端，连接名为/spawn的service ros::service::waitForService("/spawn"); // 阻塞等待/spawn的服务 ros::ServiceClient add_turtle = n.serviceClient("/spawn"); // 初始化turtlesim::Spawn的请求数据 turtlesim::Spawn srv; srv.request.x = 2.0; srv.request.y = 2.0; srv.request.name = "turtle2"; ROS_INFO("Call service to spawn turtle[x:%0.6f, y:%0.6f, name:%s]", srv.request.x, srv.request.y, srv.request.name.c_str()); // 请求服务调用 add_turtle.call(srv); // 发出请求后，阻塞等待服务器的反馈 ROS_INFO("Spawn turtle successfully [name:%s]", srv.request.name.c_str()); return 0; } ``` 完成后在CMakelists.txt中添加编译配置，catkin_make重新编译后，使用rosrun+功能包+可执行文件即可运行。 ## Server服务端实现 ```ditaa {cmd=true args=["-E"}} +-----------------+ + + + ROS Master{s}+ +-----------------+ ^ | | +-----------------------------------+--------------------------------------------+ |Registration Registration| | | | | v v +---------------+ Request +-----------------+ Request +-----------------------+ + Client + ------------------> + Service + ------------------> + Server + + + std_srvs.Trigger + {io} + std_srvs.Trigger + + + (Terminal) + <------------------ +(/turtle_command)+ <------------------ +(turtle_command_server)+ +---------------+ Response +-----------------+ Response +-----------------------+ ``` **how to do:** - 初始化ROS节点； - 创建Server实例； - 循环等待服务请求，进入回调函数 - 在回调函数中完成服务功能的处理，并反馈应答数据。 ```cpp #include #include #include ros::Publisher turtle_vel_pub; bool pubCommand = false; // service回调函数，输入参数req，输出参数res bool commandCallback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) { pubCommand = !pubCommand; ROS_INFO("Publish turtle velocity command [%s]", pubCommand == true ? "Yes" : "No"); // 设置反馈数据 res.success = true; res.message = "Change turtle command state"; return true; } int main(int argc, char **argv) { ros::init(argc, argv, "turtle_command_server"); ros::NodeHandle n; // 创建名为/turtle_command的服务，注册回调函数cammandCallback ros::ServiceServer command_service = n.advertiseService("/turtle_command", commandCallback); turtle_vel_pub = n.advertise("/turtle1/cmd_vel", 10); ROS_INFO("Ready to receive turtle command"); ros::Rate loop_rate(10); while (ros::ok()) { // 查看一次回调函数队列 ros::spinOnce(); // 标志位为true，发布速度指令 if (pubCommand) { geometry_msgs::Twist vel_msg; vel_msg.linear.x = 0.5; vel_msg.angular.z = 0.2; turtle_vel_pub.publish(vel_msg); } loop_rate.sleep(); } return 0; } ``` 完成后在CMakelists.txt中添加编译配置，catkin_make重新编译后，使用rosrun+功能包+可执行文件即可运行。 ## 服务数据的定义与使用 ```ditaa {cmd=true args=["-E"}} +-----------------+ + + + ROS Master{s}+ +-----------------+ ^ | | +--------------------------------------------+--------------------------------------------+ |Registration Registration| | | | | v v +---------------+ Request +------------------+ Request +---------------+ + Client + ------------------------> + Service + ------------------------> + Server + + + learning_service.Person + {io} + learning_service.Person + + +(person_client)+ <------------------------ + (/show_person) + <------------------------ +(person_server)+ +---------------+ Response +------------------+ Response +---------------+ ``` ### 编写自定义服务数据(Person.srv) **how to do:** - 定义.srv文件,文件放置于功能包根目录的msg文件夹中（msg/Person.srv） ``` string name uint8 gender uint8 age uint8 unkown = 0 uint8 male = 1 uint8 female = 2 --- string result ``` - 在功能包中package.xml中添加功能包依赖 ```xml message_generation message_runtime ``` - 在CMakelists.txt中添加编译选项 ```cmake ## Find catkin macros and libraries find_package(catkin REQUIRED COMPONENTS ... message_generation # 自定义消息数据需要 ) ################################################ ## Declare ROS messages, services and actions ## ################################################ #和自定义消息文件连接 add_service_files(FILES Person.srv) generate_messages(DEPENDENCIES std_msgs) ################################### ## catkin specific configuration ## ################################### catkin_package( # INCLUDE_DIRS include # LIBRARIES learning_topic CATKIN_DEPENDS geometry_msgs roscpp rospy std_msgs turtlesim message_runtime # DEPENDS system_lib ) ``` - 在工作空间中重新编译(编译之后，devel/include文件夹下会生成三个自定义srv文件对应头文件：xxx.h/xxxRequest.h/xxxResponse.h，包含时只用引用第一个头文件) ```sh cd ~/catkin_ws catkin_make ``` ### 调用自定义服务数据 client和server的实现和上文服务端/客户端流程相同，唯一区别处在于服务的名称是自定义的 **("/show_person")**，只需两者对应即可实现自定义服务的请求和调用。 client: ```cpp ros::service::waitForService("/show_person"); ros::ServiceClient person_client = n.serviceClient("/show_person"); ``` server: ```cpp ros::ServiceServer person_service = n.advertiseService("/show_person", Callback); ``` 编写完功能包代码后，在CMakelists.txt中添加编译配置项: ```cmake add_executable(person_client src/person_client.cpp) target_link_libraries(person_client ${catkin_LIBRARIES}) # 依赖动态生成的cpp文件 add_dependencies(person_client ${PROJECT_NAME}_gencpp) add_executable(person_server src/person_server.cpp) target_link_libraries(person_server ${catkin_LIBRARIES}) add_dependencies(person_server ${PROJECT_NAME}_gencpp) ``` 在工作空间下使用catkin_make重新编译 运行功能包： ```sh # 开启rosmaster roscore # 运行服务端 rosrun learning_service person_server # 运行客户端 rosrun learning_service person_client ``` --- # 参数的使用和与编程方法