ROS发布/订阅Float64MultiArray数组类消息（C++和Python相互发布和订阅）

在terminal用指令发布一个Float64MultiArray消息，这个在调试时特别好用，注意格式，最后一行的data: [1,2,3]中冒号和后面的中括号间要有空格

rostopic pub  /r2d2_gripper_controller/command std_msgs/Float64MultiArray "layout:
  dim:
  - label: ''
    size: 3
    stride: 1
  data_offset: 0
data: [-0.4, 0, 0]"

2021.05.28

---

再次领略了ROS系统一个很好的特性，能兼容不同语言的节点，只要规范好发布信息的类型是匹配的，就能让不同语言编写的节点互相通信。

Target：

想让python写的node发布数组，而让C++写的node订阅这个发布的数据。

Method:

1.Python发布，C++订阅

1）先选择发布消息的类型，选定为Float64MultiArray
2）然后是python节点的代码：

#!/usr/bin/env python
# encoding: utf-8
# Echo client program


import time

import rospy
import time
from std_msgs.msg import Int8, String, Bool, Float64MultiArray

rospy.init_node('szinter')
path_ = []
angle_=[]
vector_=[]
doubled = Int8()
doubled = 0
estop_sign = Bool()
estop_sign = False
base_signal = Int8()
base_signal = 0

COUNT = 0




def readin():
    global path_, angle_, vector_
    with open('point.txt', 'r') as f:
        data = f.readlines()

        for line in data:
            odom = line.split()
            Line = list(map(float, odom))
            path_.append(Line)

    with open('angle.txt', 'r') as f:
        data = f.readlines()

        for line in data:
            odom = line.split()
            Line = list(map(float, odom))
            angle_.append(Line)

    with open('vector.txt', 'r') as f:
        data = f.readlines()

        for line in data:
            odom = line.split()
            Line = list(map(float, odom))
            vector_.append(Line)



def inter():
    global doubled
    global base_signal
    global COUNT
    global estop_sign
    global path_, angle_, vector_

 #   rospy.Subscriber('base_state', Int8, callback1)
 #   rospy.Subscriber("e_stop", Bool, callback2)
    pub = rospy.Publisher('robot_pose', Float64MultiArray, queue_size=10)
    rate = rospy.Rate(10)  # 10hz
    readin()
    #NUM = len(URpathplanningdata)

    i=0

    while not rospy.is_shutdown():

        time.sleep(5)

        print ('robot pose')
        uposx=path_[i][0]
        uposy=path_[i][1]
        uposz=path_[i][2]
        uox=angle_[i][0]
        uoy=angle_[i][1]
        uoz=angle_[i][2]
        uvx=vector_[i][0]
        uvy=vector_[i][1]
        uvz=vector_[i][2]

        array = [uposx,uposy,uposz,uox,uoy,uoz,uvx,uvy,uvz]
        left_top = Float64MultiArray(data=array)
        pub.publish(left_top)
        i+=1

        rate.sleep()

    rospy.spin()


if __name__ == '__main__':
    try:
        inter()
    except rospy.ROSInterruptException:
        pass

3）然后是C++节点的代码：

#include "ros/ros.h"
#include "std_msgs/Float64MultiArray.h" //#include "std_msgs/String.h"
vector<double> array_from_py={0,0,0,0,0,0,0,0,0};
void myCallback(const std_msgs::Float64MultiArray& message_holder) //subscriber节点与publishers节点有一个很大不同就是需要有一个回调函数即callback function
{ 
    ROS_INFO("received value of position: %f %f %f",message_holder.data.at(0),message_holder.data.at(1),message_holder.data.at(2));
    ROS_INFO("euler angles: %f %f %f",message_holder.data.at(3),message_holder.data.at(4),message_holder.data.at(5)); 
    ROS_INFO("Vector: %f %f %f",message_holder.data.at(6),message_holder.data.at(7),message_holder.data.at(8)); 

    array_from_py[0] = message_holder.data.at(0); array_from_py[1] = message_holder.data.at(1); array_from_py[2] = message_holder.data.at(2);
    array_from_py[3] = message_holder.data.at(3); array_from_py[4] = message_holder.data.at(4); array_from_py[5] = message_holder.data.at(5); 
    array_from_py[6] = message_holder.data.at(6); array_from_py[7] = message_holder.data.at(7); array_from_py[8] = message_holder.data.at(8); 
} 

int main(int argc, char **argv)
{
  ros::init(argc, argv, "listener");

  ros::NodeHandle n;

  ros::Subscriber sub = n.subscribe("robot_pose", 1, myCallback);

  ros::spin();

  return 0;
}

注意其中获取订阅消息的方法：
①先定义一个全局变量

vector<double> array_from_py={0,0,0,0,0,0,0,0,0};

②然后在用at(0)的方式来获取对应的value

    ROS_INFO("received value of position: %f %f %f",message_holder.data.at(0),message_holder.data.at(1),message_holder.data.at(2));

2.C++发布，Python订阅

1）先选择发布消息的类型，选定为Float64MultiArray
2）然后是C++节点的代码：

#include"ros/ros.h" 
#include"std_msgs/Float64MultiArray.h"          
#include"std_msgs/String.h" 
#include<sstream>
#include<vector>
#include<string.h> 
using namespace std; 

vector<double> array_test={0,1,2,3,4,5,6,7,8,9,10,11};

int number_array()
{
    for(int k=0;k<2;k++)
    {
	    array_test[k]=k;
	}	
}
int main(int argc, char** argv) 
{ 	
	ros::init(argc, argv, "talker"); // 节点名称 
	ros::NodeHandle n; 
	ros::Publisher chatter_pub = n.advertise<std_msgs::Float64MultiArray>("chatter", 1000); 
	//ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter",1000); 
	ros::Rate loop_rate(10); 
	int count = 0; 
	while( ros::ok() )
	 { 	
		number_array();
		std_msgs::Float64MultiArray msg;
       		msg.data = array_test;
	    ROS_INFO("data: %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f%6.2f\n",array_test[0],array_test[1],array_test[2],array_test[3],array_test[4],array_test[5],array_test[6],array_test[7],array_test[8],array_test[9],array_test[10],array_test[11]); 
		chatter_pub.publish(msg); 
		ros::spinOnce(); 
		loop_rate.sleep(); 
		++count; 
	} 
	return 0; 
}

3）然后是python节点的代码：

#!/usr/bin/env python 
import rospy 
from std_msgs.msg import Float64MultiArray 
def callback(msgg):  #msgg的名字随便定义,保持一致即可
	rospy.loginfo("I heared:%6.2f %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f\n",msgg.data[0],msgg.data[1],msgg.data[2],msgg.data[3],msgg.data[4],msgg.data[5],msgg.data[6],msgg.data[7],msgg.data[8],msgg.data[9],msgg.data[10],msgg.data[11]) #数组的大小要与talker里面的一样
	
def listener(): 
	rospy.init_node('listener', anonymous= True)
	rospy.Subscriber("chatter", Float64MultiArray, callback) 
	rospy.spin() 
if __name__ == '__main__': 
	listener() 

---

还要注意：
运行Python节点前，需要先授权,在terminal输入：
chmod +x listener.py
注意"+"前和"x"后都有一个空格。

同时，也别忘了在CMakeLists文件里面加入对应的C++文件，生成可执行文件

---

在调用python时，也遇到一个关于map()函数的问题，应该是版本的问题

python3 TypeError: 'map' object is not subscriptable

解决方法：
map(apply_filters_to_token, sentences)
修改，add “list” to map，如：
return list(map(apply_filters_to_token, sentences))

3.C++发布，C++订阅

1） 通过topic 发布一段Float64MultiArray类型的数据
先上代码，如下：

 ros::Publisher pub = n.advertise<std_msgs::Float64MultiArray>("robot_pose",1);  
 std_msgs::Float64MultiArray pose_msg;
 pose_msg.data.resize(4); // 这个是设置data大小的
  pose_msg.data[0]=x(0);
  pose_msg.data[1]=x(1);
  pose_msg.data[2]=x(2);
  pose_msg.data[3]=x(3);
  pub.publish(message_2);

也有人用push_back来发送，但是我用上面这个成功了，下面这个没成功，有时间再研究一下，之后补充

用multiarray也有不好的地方，发送的数据一旦多了，在写订阅消息的时候，容易搞乱，给后期debug添加不少麻烦

2）订阅Float64MultiArray消息
先上代码，如下：

vector<double> array_from_py={0,0,0,0,0,0};

void myCallback(const std_msgs::Float64MultiArray& message_holder) 
{ 
    ROS_INFO("received value of position: %f %f %f",message_holder.data.at(0),message_holder.data.at(1),message_holder.data.at(2));
    ROS_INFO("Vector: %f %f %f",message_holder.data.at(6),message_holder.data.at(7),message_holder.data.at(8)); 

    array_from_py[0] = message_holder.data.at(0); array_from_py[1] = message_holder.data.at(1); array_from_py[2] = message_holder.data.at(2);
    array_from_py[3] = message_holder.data.at(6); array_from_py[4] = message_holder.data.at(7); array_from_py[5] = message_holder.data.at(8); 
} 

int main(int argc, char *argv[]) {

    ros::Subscriber my_subscriber_object= n.subscribe("robot_pose",1,myCallback); 

	...

要记录接收到的信息，加上一个全局变量就可以了，然后在循环中就能不停的调用接收到的Float64MultiArray消息。

Reference

1. ROS发布和订阅数组消息/ C++ & Python：
   https://blog.csdn.net/zxx258369/article/details/95453368?utm_medium=distribute.pc_relevant.none-task-blog-baidujs_title-0&spm=1001.2101.3001.4242
2. ROS发布、订阅Float64MultiArray数组类消息（C++）：
   https://blog.csdn.net/weixin_38172545/article/details/105902925
3. ROS发布Float32MultiArray消息C++/Python：
   https://blog.csdn.net/qq_30460905/article/details/88941899
4. python3 TypeError: ‘map’ object is not subscriptable：
   https://blog.csdn.net/mingyuli/article/details/81238858
   5.python3 TypeError: ‘map’ object is not subscriptable：
   https://blog.csdn.net/sunshinezhihuo/article/details/79891481