时光荏苒，珍惜当下

机器人搭建与调试7：机器人启动功能包 robot_bringup

发表于 更新于 分类于 Valine：

  机器人启动功能包 robot_bringup主要整合各类launch启动文件，仅需一个命令就能启动机器人。

创建 robot_bringup 功能包

$ catkin_create_pkg robot_bringup std_msgs rospy roscpp

转换里程计消息格式

arduino/odom 消息格式为 nav_msgs/Odometry

robot_pose_ekf/odom_combined 消息格式为 geometry_msgs/PoseWithCovarianceStamped

  • 创建robot_bringup/launch/odom_ekf.py文件
odom_ekf.py 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
#!/usr/bin/env python

import rospy
from geometry_msgs.msg import PoseWithCovarianceStamped
from nav_msgs.msg import Odometry

class OdomEKF():
    def __init__(self):
        # Give the node a name
        rospy.init_node('odom_ekf_node', anonymous=False)

        # Publisher of type nav_msgs/Odometry
        self.ekf_pub = rospy.Publisher('output', Odometry, queue_size=20)
        
        # Wait for the /odom_combined topic to become available
        rospy.wait_for_message('input', PoseWithCovarianceStamped)
        
        # Subscribe to the /odom_combined topic
        rospy.Subscriber('input', PoseWithCovarianceStamped, self.pub_ekf_odom)
        
        rospy.loginfo("Publishing combined odometry on /odom_ekf")
        
    def pub_ekf_odom(self, msg):
        odom = Odometry()
        odom.header = msg.header
        odom.header.frame_id = '/odom'
        odom.child_frame_id = 'base_link'
        odom.pose = msg.pose
        
        self.ekf_pub.publish(odom)
        
if __name__ == '__main__':
    try:
        OdomEKF()
        rospy.spin()
    except:
        pass
  • 创建 robot_bringup/launch/odom_ekf.launch 文件
odom_ekf.launch
1
2
3
4
5
6
<launch>
  <node pkg="robot_bringup" type="odom_ekf.py" name="odom_ekf" output="screen">
    <remap from="input" to="/robot_pose_ekf/odom_combined"/>
    <remap from="output" to="/odom_ekf"/>
  </node>
</launch>

创建小车启动运行文件

  • 创建robot_bringup/launch/robot_bringup.launch 文件
robot_bringup.launch
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
<!--小车运行文件-->

<launch>
    <!-- (1) 加载机器人模型 -->
    <include file="$(find robot_description)/launch/trobot_xacro.launch" />

    <!-- (2) 加载 base_controller -->
    <include file="$(find ros_arduino_python)/launch/arduino.launch" />

    <!-- (3) 加载 razor_imu -->
    <include file="$(find razor_imu_9dof)/launch/razor-pub.launch" />

    <!-- (4) 加载里程计融合 robot_pose_ekf  -->
	<node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
		<param name="output_frame" value="odom"/>
		<param name="base_footprint_frame" value="base_link"/>
		<param name="freq" value="30.0"/>
		<param name="sensor_timeout" value="1.0"/>
		<param name="odom_used" value="true"/>
		<param name="imu_used" value="true"/>
		<param name="vo_used" value="false"/>
		<param name="debug" value="false"/>
		<param name="self_diagnose" value="false"/>
		<remap from="imu_data" to="imu" />
		<!-- 将节点订阅的 imu_data 话题改名为 imu,如果 imu 节点发布的话题是 imu_data 就不用修改 -->
	</node>

   <!-- (5) 加载odom_ekf -->
    <include file="$(find robot_bringup)/launch/odom_ekf.launch"/>
完
+