源码学习：amr-lmpcc_03

本文最后更新于：2020年6月28日 上午

package:mobile_robot_state_publisher

3.1 launch文件

1
2
3
4
/amr-lmpcc/mobile_robot_state_publisher/launch
└── mobile_robot_publisher.launch

file: 1

首先是arg参数

1
<arg name="config" default="$(find mobile_robot_state_publisher)"/>

然后是用于发布机器人位姿的mobile_robot_state_publisher_node节点

1
2
3
4
<!--Publish the mobile robot pose -->
<node name="mobile_robot_state_publisher" pkg="mobile_robot_state_publisher" type="mobile_robot_state_publisher_node" respawn="false" output="screen">
	<rosparam command="load" file="$(arg config)/config/jackal.yaml" />
</node>

小结：

该文件只启动了一个节点

  • mobile_robot_state_publisher_node

3.2 jackal.yaml

在启动mobile_robot_state_publisher_node节点时加载的配置文件，主要包括：

发布的频率rate，底盘坐标系base_frame，地图坐标系root_frame，发布的机器人状态的话题名robot_state_topic和订阅的里程计的话题名vel_state_topic。

1
2
3
4
5
6
7
8
9
10
## Driver Parameters

#publisher rate
rate: 40
#robot base frame
base_frame: base_link
#map frame
root_frame: map
robot_state_topic: /robot_state
vel_state_topic: /odometry/filtered

3.3 头文件

1
2
3
4
\amr-lmpcc\mobile_robot_state_publisher\include\mobile_robot_state_publisher
└── mobile_robot_state_publisher.h

file: 1

3.3.1 class MobileRobotStatePublisher

在头文件，定义了一个类MobileRobotStatePublisher。

类的开始定义了ROS句柄nh_，订阅者jointstate_sub_，发布者robot_state_pub_和用于 tf 变换的订阅者tfListener(tfBuffer)。

1
2
3
4
5
6
7
8
ros::NodeHandle nh_;

ros::Subscriber jointstate_sub_;

ros::Publisher robot_state_pub_;

tf2_ros::Buffer tfBuffer;
tf2_ros::TransformListener tfListener(tfBuffer);

然后是构造函数，析构函数和用于初始化的成员函数initialize()

1
2
3
4
5
6
7
8
MobileRobotStatePublisher(){
}

~MobileRobotStatePublisher(){

}

bool initialize();

3.4 源文件

1
2
3
4
5
\amr-lmpcc\mobile_robot_state_publisher\src
├── mobile_robot_state_publisher_node.cpp
└── mobile_robot_state_publisher_node.cpp~

file: 2

3.4.1 mobile_robot_state_publisher_node.cpp

1. main函数

主函数开头初始化mobile_robot_state_publisher_node节点，创建句柄n和订阅者robot_state_sub_

1
2
3
ros::init(argc, argv, "mobile_robot_state_publisher_node");
ros::NodeHandle n;
ros::Subscriber robot_state_sub_;

然后是用于获取jackal.yaml中的参数的代码，此处仅放一个作为示例。

❗❗❗注：这些代码应该是打算写在成员函数initialize()中的。

1
2
3
4
5
double node_rate;
if (!n.getParam(ros::this_node::getName()+"/rate", node_rate)){
	ROS_ERROR_STREAM("mobile_robot_state_publisher_node Parameter " << ros::this_node::getName()+"/rate not set");
	return 0;
}

接着，订阅和发布了一些话题。

1
2
3
4
5
robot_state_sub_ = n.subscribe(vel_state_topic, 1, VelocityCallBack);
ros::Publisher state_pub_ = n.advertise<geometry_msgs::Pose>(robot_state_topic, 10);

tf2_ros::Buffer tfBuffer;
tf2_ros::TransformListener tfListener(tfBuffer);

发布的频率设置

1
ros::Rate rate(node_rate);

中间变量的定义

1
2
3
4
//Intermidiate variables
geometry_msgs::Pose pose_msg;
double ysqr, t3, t4;
geometry_msgs::TransformStamped transformStamped;

在循环while (n.ok())中的try-catch语句，通过frame ID获取两帧之间的变换。

  • base_link -> map的坐标变换
1
2
3
4
5
6
7
8
9
10
try{
    //Get the transform between two frames by frame ID
    //void 	lookupTransform (const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const
	transformStamped = tfBuffer.lookupTransform(root_frame, base_frame, ros::Time(0));
}
catch (tf2::TransformException &ex) {
	ROS_WARN("%s",ex.what());
	ros::Duration(1.0).sleep();
	continue;
}

然后发布机器人位姿

1
2
3
4
5
6
pose_msg.orientation.z = QuatToJointAngle(transformStamped);

pose_msg.position.x = transformStamped.transform.translation.x;
pose_msg.position.y = transformStamped.transform.translation.y;
pose_msg.position.z = vel;
state_pub_.publish(pose_msg);

最后是ROS的回旋函数

1
ros::spinOnce();

2. VelocityCallBack()

订阅者robot_state_sub_的回调函数

1
2
3
void VelocityCallBack(const nav_msgs::Odometry& msg){
	vel = sqrt(pow(msg.twist.twist.linear.x,2)+pow(msg.twist.twist.linear.y,2));
}

3. QuatToJointAngle()

四元数(Quaternion)到角度的转换，atan2返回以弧度表示的 t4/t3 的反正切。

具体可参考：中国大学MOOC《机器人操作系统入门》讲义第八章附录3三维空间刚体运动

1
2
3
4
5
6
7
8
9
double QuatToJointAngle(const geometry_msgs::TransformStamped transformStamped)
{
    const double ysqr = transformStamped.transform.rotation.y * transformStamped.transform.rotation.y;
    const double t3 = +2.0 * (transformStamped.transform.rotation.w * transformStamped.transform.rotation.z
                 + transformStamped.transform.rotation.x * transformStamped.transform.rotation.y);
    const double t4 = +1.0 - 2.0 * (ysqr + transformStamped.transform.rotation.z * transformStamped.transform.rotation.z);

    return atan2(t3, t4);
};

3.5 总结

  1. 这个包只有一个节点mobile_robot_state_publisher_node，该节点订阅话题/odometry/filtered，发布话题/robot_state。
  2. 在类MobileRobotStatePublisher中定义的发布者和订阅者没有用到，是否是多余的定义。
编程
MPC ROS 源码学习

本博客所有文章除特别声明外，均采用 CC BY-SA 3.0协议 。转载请注明出处！