def move_speed(speed, duration, rate=10):
global right_wheel_pub, left_wheel_pub, right_wheel_radius, left_wheel_radius, right_wheel_direction, left_wheel_direction, external_to_internal_wheelbase_encoder_direction
right_speed = external_to_internal_wheelbase_encoder_direction * right_wheel_direction * speed / right_wheel_radius
left_speed = external_to_internal_wheelbase_encoder_direction * left_wheel_direction * speed / left_wheel_radius
right_wheel_msg = MotorControlSetpoint()
left_wheel_msg = MotorControlSetpoint()
right_wheel_msg.node_name = 'right_wheel'
right_wheel_msg.mode = MotorControlSetpoint.MODE_CONTROL_VELOCITY
left_wheel_msg.node_name = 'left_wheel'
left_wheel_msg.mode = MotorControlSetpoint.MODE_CONTROL_VELOCITY
rate_ = rospy.Rate(rate)
for i in range(int(duration * rate)):
current_time = rospy.get_rostime()
right_wheel_msg.timestamp = current_time
right_wheel_msg.velocity = right_speed
left_wheel_msg.timestamp = current_time
left_wheel_msg.velocity = left_speed
right_wheel_pub.publish(right_wheel_msg)
left_wheel_pub.publish(left_wheel_msg)
rate_.sleep()
def move(x, duration=0.5, rate=10):
global right_wheel_pos, left_wheel_pos, right_wheel_pub, left_wheel_pub, right_wheel_radius, left_wheel_radius, right_wheel_direction, left_wheel_direction, external_to_internal_wheelbase_encoder_direction
right_wheel_msg = MotorControlSetpoint()
left_wheel_msg = MotorControlSetpoint()
right_wheel_msg.node_name = 'right_wheel'
right_wheel_msg.mode = MotorControlSetpoint.MODE_CONTROL_POSITION
left_wheel_msg.node_name = 'left_wheel'
left_wheel_msg.mode = MotorControlSetpoint.MODE_CONTROL_POSITION
right_wheel_msg.position = right_wheel_pos
left_wheel_msg.position = left_wheel_pos
right_increment = (external_to_internal_wheelbase_encoder_direction * right_wheel_direction * x / right_wheel_radius) / (duration * rate)
left_increment = (external_to_internal_wheelbase_encoder_direction * left_wheel_direction * x / left_wheel_radius) / (duration * rate)
rate_ = rospy.Rate(rate)
for i in range(int(duration * rate)):
current_time = rospy.get_rostime()
right_wheel_msg.timestamp = current_time
right_wheel_msg.position += right_increment
left_wheel_msg.timestamp = current_time
left_wheel_msg.position += left_increment
right_wheel_pub.publish(right_wheel_msg)
left_wheel_pub.publish(left_wheel_msg)
rate_.sleep()
def move_speed(speed, duration, rate=10):
global right_wheel_pub, left_wheel_pub, right_wheel_radius, left_wheel_radius, right_wheel_direction, left_wheel_direction, external_to_internal_wheelbase_encoder_direction
right_speed = external_to_internal_wheelbase_encoder_direction * right_wheel_direction * speed / right_wheel_radius
left_speed = external_to_internal_wheelbase_encoder_direction * left_wheel_direction * speed / left_wheel_radius
right_wheel_msg = MotorControlSetpoint()
left_wheel_msg = MotorControlSetpoint()
right_wheel_msg.node_name = 'right_wheel'
right_wheel_msg.mode = MotorControlSetpoint.MODE_CONTROL_VELOCITY
left_wheel_msg.node_name = 'left_wheel'
left_wheel_msg.mode = MotorControlSetpoint.MODE_CONTROL_VELOCITY
rate_ = rospy.Rate(rate)
for i in range(int(duration * rate)):
current_time = rospy.get_rostime()
right_wheel_msg.timestamp = current_time
right_wheel_msg.velocity = right_speed
left_wheel_msg.timestamp = current_time
left_wheel_msg.velocity = left_speed
right_wheel_pub.publish(right_wheel_msg)
left_wheel_pub.publish(left_wheel_msg)
rate_.sleep()
def talker():
rospy.init_node("setpoint_sender")
# Initialise robot pose
pub = rospy.Publisher("/fishing_y_axis/setpoint", MotorControlSetpoint, queue_size=1)
time.sleep(1)
setpoint = 2
counter = 0
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
msg = MotorControlSetpoint()
msg.timestamp = rospy.get_rostime()
msg.node_name = "fishing_y_axis"
msg.mode = MotorControlSetpoint.MODE_CONTROL_POSITION
msg.position = setpoint + 8
rospy.loginfo("Sending setpoint")
pub.publish(msg)
rate.sleep()
counter += 1
if counter == 5:
setpoint = -setpoint
counter = 0
def talker():
rospy.init_node('setpoint_sender')
# Initialise robot pose
pub = rospy.Publisher('/fishing_y_axis/setpoint', MotorControlSetpoint, queue_size=1)
time.sleep(1)
setpoint = 2
counter = 0
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
msg = MotorControlSetpoint()
msg.timestamp = rospy.get_rostime()
msg.node_name = 'fishing_y_axis'
msg.mode = MotorControlSetpoint.MODE_CONTROL_POSITION
msg.position = setpoint + 8
rospy.loginfo('Sending setpoint')
pub.publish(msg)
rate.sleep()
counter += 1
if counter == 5:
setpoint = - setpoint
counter = 0
def move(x, duration=0.5, rate=10):
global right_wheel_pos, left_wheel_pos, right_wheel_pub, left_wheel_pub, right_wheel_radius, left_wheel_radius, right_wheel_direction, left_wheel_direction, external_to_internal_wheelbase_encoder_direction
right_wheel_msg = MotorControlSetpoint()
left_wheel_msg = MotorControlSetpoint()
right_wheel_msg.node_name = 'right_wheel'
right_wheel_msg.mode = MotorControlSetpoint.MODE_CONTROL_POSITION
left_wheel_msg.node_name = 'left_wheel'
left_wheel_msg.mode = MotorControlSetpoint.MODE_CONTROL_POSITION
right_wheel_msg.position = right_wheel_pos
left_wheel_msg.position = left_wheel_pos
right_increment = (external_to_internal_wheelbase_encoder_direction *
right_wheel_direction * x /
right_wheel_radius) / (duration * rate)
left_increment = (external_to_internal_wheelbase_encoder_direction *
left_wheel_direction * x /
left_wheel_radius) / (duration * rate)
rate_ = rospy.Rate(rate)
for i in range(int(duration * rate)):
current_time = rospy.get_rostime()
right_wheel_msg.timestamp = current_time
right_wheel_msg.position += right_increment
left_wheel_msg.timestamp = current_time
left_wheel_msg.position += left_increment
right_wheel_pub.publish(right_wheel_msg)
left_wheel_pub.publish(left_wheel_msg)
rate_.sleep()