Trying to transmit a 1 by 4 array message to subscriber
I am having trouble trying to publish correctly a 1 by 4 array message generated using a for loop based on test data. My system is a ubuntu 16.04 running Kinetic.
The error occurred in the pub.publish(message) line. Below is my code:
# Import Packages
import sys
sys.path.remove('/opt/ros/kinetic/lib/python2.7/dist-packages')
from raisim_gym.algo.ppo2 import PPO2
import os
import math
import argparse
import pickle
def action2Thrust(mylist):
_actionMean = 2.5
_actionStd = 2
Newthrust = mylist.astype(float)*_actionStd
Newthrust += _actionMean
return Newthrust
# Have to add the path again so that import rospy works.
sys.path.append('/opt/ros/kinetic/lib/python2.7/dist-packages')
import rospy
from std_msgs.msg import String
from transmit_thrust.msg import ThrustAction
# Function to map the action value to true thrust range none negative to the range between
# 0.5 to 4.5 scale
if __name__ == '__main__':
try:
# configuration
parser = argparse.ArgumentParser()
parser.add_argument('-w', '--weight', help='trained weight path', type=str, default='')
args = parser.parse_args()
weight_path = args.weight
if weight_path == "":
print("Can't find trained weight, please provide a trained weight with --weight switch\n")
else:
print("Loaded weight from {}\n".format(weight_path))
model = PPO2.load(weight_path)
# Needed to load observations here to test this script would be using pickle here
with open('/home/ted/raisim_workspace/raisimGym/scripts/obArray.pickle', 'rb') as f:
obs = pickle.load(f)
# Needed to instantiate an array to save Thrust for outputting
ThrustArray = ThrustAction()
pub = rospy.Publisher('chatter', ThrustAction, queue_size=10)
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(100) # 10hz
for i in range(len(obs)):
action, _ = model.predict(obs[i])
# print("Action: {}".format(action))
Thrust = action2Thrust(action)
# print("Thrust: {}".format(Thrust))
ThrustArray = Thrust
while not rospy.is_shutdown():
rospy.loginfo(ThrustArray)
pub.publish(ThrustArray)
rate.sleep()
except rospy.ROSInterruptException:
pass
The obs obtain a set of test data in a large array in the form of 18 by 47093, which is 18 columns and 47093 rows. The arrays are fed into a for loop generating individual thrust command of 1 by 4 which are published to the subscriber node. I have tried to replace the line:
ThrustArray = Thrust
to
ThrustArray.action = Thrust
where action is the attribute of ThrustArray.msg which is defined as a float32[]. Below is the error output:
[INFO] [1569343371.533475]: [[0.5
3.76598799 0.6819396 0.91531122]] Traceback (most recent call last):
File "/home/ted/catkin_ws/devel/lib/python2.7/dist-packages/transmit_thrust/msg/_ThrustAction.py", line 55, in serialize buff.write(struct.pack(pattern, *self.action)) struct.error: required argument is not a float
During handling of the above exception, another exception occurred:
Traceback (most recent call last):
File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/topics.py", line 882, in publish self.impl.publish(data) File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/topics.py", line 1066, in publish serialize_message(b, self.seq, message) File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/msg.py", line 152, in serialize_message msg.serialize(b) File ...
Ok, so what returns
model.predict(obs[i])
? It seems to me it is a numpy array, as you useastype(float)
laster on with it, right? And what exactly is your message definition? Since you asked, if there is something wrong with defining your custom messageHi LeoE,
Yes you are right, model.predict(obs[i]) returns a 1 by 4 numpy array in the form like [1 1 -1 -1], where the numbers are in between -1 to 1. It could have many decimal places that's why I assigned ThrustAction.msg as following:
where action is the attribute of ThrustAction as my custom message file. I did this following this question on stackflow Publishing Array
It seemed to me the resulting output ThrustAction had two square brackets but I only needed one to be a list. I am not sure why this is. Or it could be that I needed to assign float64[] instead, but I highly doubt it.
I think it's highly likely that I needed to numpize my array before publishing since as LeoE pointed out that my arrays are numpy arrays not lists. So I will follow the tutorial here and see if it works: Numpize Array