def main(args): node_name = 'youbot_velocity_control' rospy.init_node(node_name) print('Node started') base_publisher = rospy.Publisher('/cmd_vel', geometry_msgs.msg.Twist) arm1_publisher = rospy.Publisher('/arm_1/arm_controller/velocity_command', brics_actuator.msg.JointVelocities) def velocity_array_callback(msg): print('Received array ' + str(msg.data)) base_msg = get_twist_velocity_msg(msg.data[:3], rospy.get_rostime()) arm_msg = get_joint_velocity_msg(msg.data[3:], rospy.get_rostime()) base_publisher.publish(base_msg) arm1_publisher.publish(arm_msg) # msg = get_joint_velocity_msg(msg.data, rospy.get_rostime()) # velocity_publisher.publish(msg) rospy.Subscriber('/youbot/velocity_instruction', array_msgs.msg.FloatArray, velocity_array_callback) def stop_manipulator(): print('Shutting down') # Finally stop the robot base_msg = get_twist_velocity_msg([0, 0, 0], rospy.get_rostime()) arm_msg = get_joint_velocity_msg([0, 0, 0, 0, 0], rospy.get_rostime()) base_publisher.publish(base_msg) arm1_publisher.publish(arm_msg) print('Signal stop sent') rospy.on_shutdown(stop_manipulator) while not rospy.is_shutdown(): try: sys.stdout.write('\033[45m' + node_name + '$\033[0m ') evaled = eval(sys.stdin.readline()) print(str(evaled)) # pdb.set_trace() if evaled.__class__ in [tuple, list]: # print('\033[91mError: Command not supported\033[0m') array = np.array(evaled) base_msg = get_twist_velocity_msg(array[:3], rospy.get_rostime()) arm_msg = get_joint_velocity_msg(array[3:], rospy.get_rostime()) base_publisher.publish(base_msg) arm1_publisher.publish(arm_msg) print('Published velocity command: ' + str(array)) else: print('\033[91mError: Unexpected command\033[0m') base_msg = get_twist_velocity_msg([0, 0, 0], rospy.get_rostime()) arm_msg = get_joint_velocity_msg([0, 0, 0, 0, 0], rospy.get_rostime()) base_publisher.publish(base_msg) arm1_publisher.publish(arm_msg) except: print('\033[91mError: Unexpected error occurred \033[0m') base_msg = get_twist_velocity_msg([0, 0, 0], rospy.get_rostime()) arm_msg = get_joint_velocity_msg([0, 0, 0, 0, 0], rospy.get_rostime()) base_publisher.publish(base_msg) arm1_publisher.publish(arm_msg)
def stop_manipulator(): print('Shutting down') # Finally stop the robot base_msg = get_twist_velocity_msg([0, 0, 0], rospy.get_rostime()) arm_msg = get_joint_velocity_msg([0, 0, 0, 0, 0], rospy.get_rostime()) base_publisher.publish(base_msg) arm1_publisher.publish(arm_msg) print('Signal stop sent')
def velocity_array_callback(msg): print('Received array ' + str(msg.data)) base_msg = get_twist_velocity_msg(msg.data[:3], rospy.get_rostime()) arm_msg = get_joint_velocity_msg(msg.data[3:], rospy.get_rostime()) base_publisher.publish(base_msg) arm1_publisher.publish(arm_msg)