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 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)
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 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)
