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 main(args): node_name = 'youbot_arm_velocity_demo' rospy.init_node(node_name) print('Node started') velocity_publisher = rospy.Publisher('/arm_1/arm_controller/velocity_command', brics_actuator.msg.JointVelocities) def stop_manipulator(): print('Shutting down') # Finally stop the robot msg = get_joint_velocity_msg(np.zeros(5), rospy.get_rostime()) velocity_publisher.publish(msg) print('Signal stop sent') rospy.on_shutdown(stop_manipulator) t0 = rospy.get_time() T = 5 # Wave length ampl = np.array([.1, 0, 0, 0, 0]) offs = np.array([1, 0, 0, 0, 0]) pdb.set_trace() r = rospy.Rate(5) msg = get_joint_velocity_msg(ampl + offs) velocity_publisher.publish(msg) for _ in range(5): r.sleep() print('In position, starting the demo.') while not rospy.is_shutdown(): t = rospy.get_time() array = ampl * np.sin((t - t0) * (2 * np.pi) / T) msg = get_joint_velocity_msg(array, rospy.Time.from_sec(t)) print(array) velocity_publisher.publish(msg) r.sleep()
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)
def stop_manipulator(): rospy.logerr('Sending stop signal (zero velocity) to robot arm') msg = get_joint_velocity_msg(np.zeros(5), rospy.get_rostime()) velocity_publisher.publish(msg)
def stop_manipulator(): print('Shutting down') # Finally stop the robot msg = get_joint_velocity_msg(np.zeros(5), rospy.get_rostime()) velocity_publisher.publish(msg) print('Signal stop sent')
def stop_manipulator(): print('Sending stop signal (zero velocity) to robot arm') # Finally stop the robot msg = get_joint_velocity_msg(np.zeros(5), rospy.get_rostime()) velocity_publisher.publish(msg)