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