def axis_states_publish(axes):
try:
msg_arr = msg.Float32MultiArray()
msg_arr.data = axes
msg_arr.layout.dim = [
msg.MultiArrayDimension(),
msg.MultiArrayDimension(),
msg.MultiArrayDimension()
]
msg_arr.layout.dim[0].size = len(axes)
msg_arr.layout.dim[1].size = len(axes[0])
msg_arr.layout.dim[2].size = len(axes[0][0])
msg_arr.layout.dim[0].stride = len(axes) * len(axes[0]) * len(
axes[0][0])
msg_arr.layout.dim[1].stride = len(axes[0]) * len(axes[0][0])
msg_arr.layout.dim[2].stride = len(axes[0][0])
msg_arr.layout.dim[0].label = "odrives"
msg_arr.layout.dim[1].label = "axes"
msg_arr.layout.dim[2].label = "values"
# print(msg_arr.data)
# print(msg_arr)
axis_state_pub.publish(msg_arr)
# rospy.loginfo("Sent on debug: " + str(axes))
except rospy.ROSInterruptException:
pass
def flippers_position_publish(pos):
try:
msg_arr = msg.Float32MultiArray(data=pos)
flippers_velocity_pub.publish(msg_arr)
except rospy.ROSInterruptException:
pass
def drive_distance_publish(pos):
try:
msg_arr = msg.Float32MultiArray(data=pos)
drive_distance_pub.publish(msg_arr)
except rospy.ROSInterruptException:
pass
def drive_velocity_publish(vel):
try:
print("Starting to publish drive velocity...")
msg_arr = msg.Float32MultiArray(data=vel)
drive_velocity_pub.publish(msg_arr)
print("Drive velocity publshed! " + str(drive_velocity_pub.get_num_connections()) + " nodes should have heard it")
except rospy.ROSInterruptException:
pass
def talker(p_traj,v_traj,a_traj,t0,tf):
pub = rospy.Publisher('plot', plot_data, queue_size = 10)
rospy.init_node('plot_cubic_traj', anonymous=True)
rate = rospy.Rate(0.1)
# msg = send
T = np.arange(t0,tf,0.01)
# for _T,_p_traj in zip(T,p_traj):
# _pub = [_T,_p_traj]
# pub.publish(_pub)
# rospy.loginfo(p_traj)
p_out = std_msgs.Float32MultiArray()
p_out.layout = T
p_out.data = p_traj
out_msg = plot_data()
if(len(p_traj)>2):
plt.figure(0)
plt.clf()
plt.plot(T,p_traj)
plt.plot(T,v_traj)
plt.plot(T,a_traj)
out_msg.p_out = p_traj
out_msg.a_out = v_traj
out_msg.a_out = a_traj
for _i in range(len(T)):
out_msg.p_out = p_traj[_i]
out_msg.a_out = v_traj[_i]
out_msg.a_out = a_traj[_i]
pub.publish(out_msg)
plt.pause(3)
plt.close(0)
# p_out.data = v_traj
# pub.publish(v_traj)
# p_out.data = a_traj
# pub.publish(a_traj)
rate.sleep()
def _send_movement_message(self, btn_text):
message = msg.Float32MultiArray()
if (btn_text == "Forward"):
self.create_alert('You clicked the {0} button!'.format(
btn_text.lower()))
message.data = [1, 1]
elif (btn_text == "Backward"):
self.create_alert('You clicked the {0} button!'.format(
btn_text.lower()))
message.data = [-1, -1]
elif (btn_text == "Left"):
self.create_alert('You clicked the {0} button!'.format(
btn_text.lower()))
message.data = [0, 1]
else:
self.create_alert('You clicked the {0} button!'.format(
btn_text.lower()))
message.data = [1, 0]
self.publishers['drive/distance'].publish(message)