建模实践6——创建 position_publisher 节点

之前的husky_kinova平台已经搭建完毕，但是发送位置命令时，都是采用 rostopic pub 的方式发送的：

rostopic pub -r 10 /joint_1_position_controller/command std_msgs/Float64 "data: 3.0"

如果后续设计轨迹规划算法，这样的方式显然不再可行，为此，这里练习使用 C++ 编写 publisher 节点来取代该方式。

1. 创建功能包

功能包是源码的最小组织单元，在编写代码前要先创建功能包。

hjs@joker:~$ cd catkin_ws/src/husky_kinova
hjs@joker:~/catkin_ws/src/husky_kinova$ catkin_create_pkg position_control_pkg roscpp rospy std_msgs
Created file position_control_pkg/package.xml
Created file position_control_pkg/CMakeLists.txt
Created folder position_control_pkg/include/position_control_pkg
Created folder position_control_pkg/src
Successfully created files in /home/hjs/catkin_ws/src/husky_kinova/position_control_pkg. Please adjust the values in package.xml.

2. 编译功能包

回到工作空间目录下，编译

hjs@joker:~/catkin_ws/src/husky_kinova$ cd ../..
hjs@joker:~/catkin_ws$ catkin_make

3. C++编写Publisher节点

/*
该程序将发布 joint_3_position_controller/command 话题，消息类型为 std_msgs/Float64 
*/
#include <ros/ros.h>
#include <std_msgs/Float64.h>

int main(int argc, char **argv)
{
	// ROS节点初始化,定义节点名称为 position_publisher，注意节点名称不能重复，告诉ROS Master我来了
	ros::init(argc, argv, "position_publisher");

	// 创建节点句柄，用来管理节点资源
	ros::NodeHandle n;

	// 创建一个Publisher，发布名为 /joint_3_position_controller/command 的topic，消息类型为std_msgs::Float，队列长度10
	ros::Publisher kinova_pos_pub = n.advertise<std_msgs::Float64>("/joint_3_position_controller/command", 10);

	// 设置循环的频率
	ros::Rate loop_rate(10);

	int count = 0;
	while (ros::ok())
	{
	    // 初始化std_msgs::Float64类型的消息
		std_msgs::Float64 pos_msg;
		pos_msg.data = 3.14;

	    // 发布消息
		kinova_pos_pub.publish(pos_msg);
		ROS_INFO("Publish kinova position command[%f rad]", pos_msg);

	    // 按照循环频率延时
	    loop_rate.sleep();
	}

	return 0;
}

4. 配置 CMakeLists.txt 中的编译规则

1、设置需要编译的代码和生成的可执行文件
2、设置链接库

####################################################################
add_executable(position_publisher src/position_publisher.cpp)

target_link_libraries(position_publisher
    ${catkin_LIBRARIES}
)
#####################################################################

5. 回到工作空间下，编译

cd catkin_ws
catkin_make
source devel/setup.bash

6. 运行该节点

先运行husky_kinova的gazebo仿真环境

hjs@joker:~/catkin_ws$ roslaunch husky_kinova_gazebo husky_kinova_empty_world.launch 

新开一个terminal，运行position_publisher节点

hjs@joker:~/catkin_ws$ rosrun position_control_pkg position_publisher 
[ INFO] [1609594848.897082906]: Publish kinova position command[3.140000 rad]
[ INFO] [1609594849.274046971, 359.377000000]: Publish kinova position command[3.140000 rad]
[ INFO] [1609594849.274092545, 359.377000000]: Publish kinova position command[3.140000 rad]
[ INFO] [1609594849.420326444, 359.478000000]: Publish kinova position command[3.140000 rad]
[ INFO] [1609594849.562270646, 359.577000000]: Publish kinova position command[3.140000 rad]
[ INFO] [1609594849.714226713, 359.681000000]: Publish kinova position command[3.140000 rad]
[ INFO] [1609594849.850372516, 359.777000000]: Publish kinova position command[3.140000 rad]
[ INFO] [1609594849.989314027, 359.878000000]: Publish kinova position command[3.140000 rad]
[ INFO] [1609594850.132852317, 359.977000000]: Publish kinova position command[3.140000 rad]
[ INFO] [1609594850.270160545, 360.078000000]: Publish kinova position command[3.140000 rad]
......

7. 运行结果

kinova 的 joint3 运行到了程序中指定的位置，实现了和 rostopic pub 同样的功能。

rqt_graph

可以看到，编写的 /position_publisher 节点正在发布 /joint_3_position_controller/command 的 topic，而该 topic 被 /gazebo 节点订阅了，因此 gazebo 中的 kinova 模型才运动到了程序中指定的位置。