def check_crashed(data):
# Check whether we have crashed or not
if not data.data:
# Drive straight until we hit something
msg = MotorCommand()
msg.joint_name = "steering"
msg.acceleration = 0
msg.speed = 1
msg.position = STEER_STRAIGHT
pololu_pub.publish(msg)
msg = MotorCommand()
msg.joint_name = "drive"
msg.acceleration = 0
msg.speed = 1
msg.position = CRASH_VELOCITY
pololu_pub.publish(msg)
else:
# Make sure to turn off crash mode
crash_mode = False
reverse_reverse()
return
def reverse_reverse():
# But first stop
msg = MotorCommand()
msg.joint_name = "drive"
msg.acceleration = 0
msg.speed = 1
msg.position = DRIVE_STOP
pololu_pub.publish(msg)
msg = MotorCommand()
msg.joint_name = "steering"
msg.acceleration = 0
msg.speed = 1
msg.position = STEER_STRAIGHT
pololu_pub.publish(msg)
# Stop for 1 second and reflect on what you have done
time.sleep(1)
# Now reverse reverse
msg = MotorCommand()
msg.joint_name = "drive"
msg.acceleration = 0
msg.speed = 1
msg.position = BACKUP_VELOCITY
pololu_pub.publish(msg)
# Put processes to sleep for 3 seconds
time.sleep(3)
# Now go back to f*****g shit up
return
def callback(self,msg):
mtr=MotorCommand()
mtr.joint_name=self.channel_name
if msg.data==True:
mtr.position=0.77
mtr.speed=1.0
mtr.acceleration=1.0
else:
mtr.position=-0.77
mtr.speed=0.0
mtr.acceleration=0.0
self.pub.publish(mtr)
def moveMotor(self, jntName, pos, speed):
mtr = MotorCommand()
mtr.joint_name = jntName
mtr.position = pos
mtr.speed = speed #/self.MaxSpeed#pololu take 0 to 1.0 as speed, check the correct division
mtr.acceleration = 1.0
self.pub.publish(mtr)
def moveMotor(self,jntName,pos,speed):
mtr=MotorCommand()
mtr.joint_name=jntName
mtr.position=pos
mtr.speed=speed#/self.MaxSpeed#pololu take 0 to 1.0 as speed, check the correct division
mtr.acceleration=1.0
self.pub.publish(mtr)
def main():
rospy.init_node('killing_motor')
motor_pub = rospy.Publisher('/pololu/command/', MC, queue_size=3)
output = MC()
output.joint_name = 'Throttle'
output.position = 0
output.speed = 1.0
output.acceleration = 1
motor_pub.publish(output)
def start_expression(self, goal_handle):
goal = goal_handle.get_goal()
expression = Expression[goal.expression]
speed = goal.speed
intensity = goal.intensity
duration = goal.duration
# Checking for default values (-1 means nothing was specified, hence we should use the default values)
if speed == -1:
speed = expression.default_speed
if intensity == -1:
intensity = expression.default_intensity
if duration == -1 and expression.default_duration is not None:
duration = expression.default_duration
if expression in [Expression.smile, Expression.frown_mouth]:
joint_name = 'smile_joint'
elif expression in [Expression.open_mouth]:
joint_name = 'jaw_joint'
elif expression in [Expression.frown]:
joint_name = 'brow_joint'
negative = True
if expression in [Expression.smile, Expression.open_mouth]:
negative = False
response = self.motor_range_srv(joint_name)
msg = MotorCommand()
msg.joint_name = joint_name
min_range = min([response.min, response.max])
max_range = max([response.min, response.max])
if negative:
msg.position = interpolate(intensity, 0.0, 1.0, 0.0, min_range)
else:
msg.position = interpolate(intensity, 0.0, 1.0, 0.0, max_range)
msg.speed = interpolate(speed, 0.0, 1.0, 0.0, 0.5)
msg.acceleration = interpolate(speed, 0.0, 1.0, 0.0, 0.4)
start = time.time()
while not rospy.is_shutdown(
) and not self.action_server.is_preempt_requested(goal_handle) and (
time.time() - start) < duration:
if duration >= 0.0 and (time.time() - start) < duration:
break
self.motor_pub.publish(msg)
self.rate.sleep()
if not self.action_server.is_preempt_requested(goal_handle):
self.action_server.set_succeeded(goal_handle)
def callback_states(self, states):
self.actuator_states = states
left_ir = states.motor_states[0].pulse
right_ir = states.motor_states[1].pulse
d_ref = 142
#d_ref = 95
#d_ref = 120
print states.motor_states[1].pulse
# Publish left IR state : distance from the wall
self.distance_pub_r.publish(states.motor_states[1].pulse)
self.distance_pub_l.publish(states.motor_states[0].pulse)
servo_cmd = MotorCommand()
self.current_time = rospy.Time.now()
#self.dt = (self.current_time - self.last_time).to_sec()
error = left_ir - right_ir
self.error_pub.publish(error)
if error < -2:
u = float(self.Kp) * error / 5.58
elif error > 2:
u = float(self.Kp) * error / 25.67
else:
u = 0
if u > 3:
u = 2.2
elif u < -17:
u = -17
# Publish steering control effort
self.controlEffort_pub.publish(u)
#print u
servo_cmd.position = u
servo_cmd.joint_name = "steering"
self.steering_pub.publish(servo_cmd)
servo_cmd.joint_name = "thrust"
servo_cmd.speed = float(0)
servo_cmd.acceleration = float(0)
if self.stop_detected == True:
servo_cmd.position = float(0)
else:
servo_cmd.position = float(0.48)
#servo_cmd.position = float(0)
self.steering_pub.publish(servo_cmd)
self.last_time = self.current_time
self.last_error = error
def start_expression(self, goal_handle):
goal = goal_handle.get_goal()
expression = Expression[goal.expression]
speed = goal.speed
intensity = goal.intensity
duration = goal.duration
# Checking for default values (-1 means nothing was specified, hence we should use the default values)
if speed == -1:
speed = expression.default_speed
if intensity == -1:
intensity = expression.default_intensity
if duration == -1 and expression.default_duration is not None:
duration = expression.default_duration
if expression in [Expression.smile, Expression.frown_mouth]:
joint_name = 'smile_joint'
elif expression in [Expression.open_mouth]:
joint_name = 'jaw_joint'
elif expression in [Expression.frown]:
joint_name = 'brow_joint'
negative = True
if expression in [Expression.smile, Expression.open_mouth]:
negative = False
response = self.motor_range_srv(joint_name)
msg = MotorCommand()
msg.joint_name = joint_name
min_range = min([response.min, response.max])
max_range = max([response.min, response.max])
if negative:
msg.position = interpolate(intensity, 0.0, 1.0, 0.0, min_range)
else:
msg.position = interpolate(intensity, 0.0, 1.0, 0.0, max_range)
msg.speed = interpolate(speed, 0.0, 1.0, 0.0, 0.5)
msg.acceleration = interpolate(speed, 0.0, 1.0, 0.0, 0.4)
start = time.time()
while not rospy.is_shutdown() and not self.action_server.is_preempt_requested(goal_handle) and (time.time() - start) < duration:
if duration >= 0.0 and (time.time() - start) < duration:
break
self.motor_pub.publish(msg)
self.rate.sleep()
if not self.action_server.is_preempt_requested(goal_handle):
self.action_server.set_succeeded(goal_handle)
def listen(self, msg):
'''string joint_name # Name of the joint (specified in the yaml file), or motor_id for default calibration
float64 position # Position to move to in radians
float32 speed # Speed to move at (0.0 - 1.0)
float32 acceleration # Acceleration to move at (0.0 - 1.0)'''
output = MC()
output.joint_name = 'Turning'
output.position = msg.data
output.speed = 0.5
output.acceleration = 0.1
self.motor_pub.publish(output)
def actuate(ang, ang_speed, ang_acc, vel, vel_speed, vel_acc):
#cmd.position: radians, -45 min (left), -45 max (right)
#speed and accel 0 to 1
cmd = MotorCommand()
cmd.joint_name = "steering"
#cmd.position = degToRad(ang)
cmd.position = ang
cmd.speed = ang_speed
cmd.acceleration = ang_acc
print("Setting angle to ", ang)
pololu_pub.publish(cmd)
#cmd.position: radians, 0 min, 45 max (1560)
#speed and accel 0 to 1
cmd.joint_name = "drive"
#cmd.position = degToRad(vel)
cmd.position = vel
cmd.speed = vel_speed
cmd.acceleration = vel_acc
print("Setting speed to ", vel)
pololu_pub.publish(cmd)
def throttle_listen(self, msg):
'''string joint_name # Name of the joint (specified in the yaml file), or motor_id for default calibration
float64 position # Position to move to in radians
float32 speed # Speed to move at (0.0 - 1.0)
float32 acceleration # Acceleration to move at (0.0 - 1.0)'''
if abs(self.throttle_last - msg.data) / (abs(msg.data) +
0.001) > PUBTHRESH:
output = MC()
output.joint_name = 'Throttle'
output.position = msg.data
output.speed = 1.0
output.acceleration = 0.5
self.motor_pub.publish(output)
self.throttle_last = msg.data
def start_expression(self, goal_handle):
goal = goal_handle.get_goal()
expression = ZoidExpression[goal.expression]
speed = goal.speed
intensity = goal.intensity
duration = goal.duration
if expression in [ZoidExpression.smile, ZoidExpression.frown_mouth]:
joint_name = 'smile_joint'
elif expression in [ZoidExpression.open_mouth]:
joint_name = 'jaw_joint'
elif expression in [ZoidExpression.frown]:
joint_name = 'brow_joint'
negative = True
if expression in [ZoidExpression.smile, ZoidExpression.open_mouth]:
negative = False
response = self.motor_range_srv(joint_name)
msg = MotorCommand()
msg.joint_name = joint_name
min_range = min([response.min, response.max])
max_range = max([response.min, response.max])
if negative:
msg.position = interpolate(intensity, 0.0, 1.0, 0.0, min_range)
else:
msg.position = interpolate(intensity, 0.0, 1.0, 0.0, max_range)
msg.speed = interpolate(speed, 0.0, 1.0, 0.0, 0.5)
msg.acceleration = interpolate(speed, 0.0, 1.0, 0.0, 0.4)
start = time.time()
while not rospy.is_shutdown(
) and not self.action_server.is_preempt_requested(goal_handle) and (
time.time() - start) < duration:
self.motor_pub.publish(msg)
self.rate.sleep()
if not self.action_server.is_preempt_requested(goal_handle):
self.action_server.set_succeeded(goal_handle)
def start_expression(self, goal_handle):
goal = goal_handle.get_goal()
expression = ZoidExpression[goal.expression]
speed = goal.speed
intensity = goal.intensity
duration = goal.duration
if expression in [ZoidExpression.smile, ZoidExpression.frown_mouth]:
joint_name = 'smile_joint'
elif expression in [ZoidExpression.open_mouth]:
joint_name = 'jaw_joint'
elif expression in [ZoidExpression.frown]:
joint_name = 'brow_joint'
negative = True
if expression in [ZoidExpression.smile, ZoidExpression.open_mouth]:
negative = False
response = self.motor_range_srv(joint_name)
msg = MotorCommand()
msg.joint_name = joint_name
min_range = min([response.min, response.max])
max_range = max([response.min, response.max])
if negative:
msg.position = interpolate(intensity, 0.0, 1.0, 0.0, min_range)
else:
msg.position = interpolate(intensity, 0.0, 1.0, 0.0, max_range)
msg.speed = interpolate(speed, 0.0, 1.0, 0.0, 0.5)
msg.acceleration = interpolate(speed, 0.0, 1.0, 0.0, 0.4)
start = time.time()
while not rospy.is_shutdown() and not self.action_server.is_preempt_requested(goal_handle) and (time.time() - start) < duration:
self.motor_pub.publish(msg)
self.rate.sleep()
if not self.action_server.is_preempt_requested(goal_handle):
self.action_server.set_succeeded(goal_handle)
#
#
import rospy
from std_msgs.msg import Float32MultiArray
# from ackermann_msgs.msg import AckermannDriveStamped
from ros_pololu_servo.msg import MotorCommand
# from sensor_msgs.msg import LaserScan
import numpy as np
import time
servo_commands = MotorCommand()
drive_commands = MotorCommand()
# DC Motor Min 0.1 Max 0.45 (0.62 but we don't see a change)
servo_commands.joint_name = 'servo' #Check name here
drive_commands.joint_name = 'motor' #Check name here
drive_commands.position = 0 #Set speed here
drive_commands.speed = 0
# acceleration for best run = .2
drive_commands.acceleration = .07
servo_commands.position = 0
servo_commands.speed = 0.2
servo_commands.acceleration = 0.05
turn_number = 0
s_angle = 0
pub = rospy.Publisher('/pololu/command', MotorCommand, queue_size=1)
#pub.publish(drive_commands)
raw_input("Press Enter to RUMBLEEEE")
#State Machine Variable
car_state = 1
max_speed = .3
def test(data):
# Check if we want to crash and backout
if crash_mode:
return
pub_gap = rospy.Publisher('ftg', LaserScan, queue_size=1)
pub_no_gap = rospy.Publisher('noGap', Bool, queue_size=10)
# It would be good here to package this angle information into the message
left_ang = np.rad2deg(data.angle_max)
right_ang = np.rad2deg(data.angle_max)
inc_ang = np.rad2deg(data.angle_increment)
depths = data.ranges
depth_thresh = DEPTH_THRESH
depths = list(depths)
for idx, depth in enumerate(depths):
if depth > depth_thresh:
depths[idx] = 0
c_depths = copy.deepcopy(depths)
start = 0
num_zeros = 0
in_gap = False
cur_zeros = 0
cur_start = 0
for idx, depth in enumerate(depths):
if not depth == 0:
depths[idx] = 1
cur_zeros = 0
in_gap = False
else:
if not in_gap:
cur_start = idx
cur_zeros = 0
in_gap = True
depths[idx] = 0.5
cur_zeros += 1
if cur_zeros > num_zeros:
num_zeros = cur_zeros
start = cur_start
middle = start + num_zeros/2
ang = -(45.0 - inc_ang * middle)
c_depths_2 = copy.deepcopy(depths)
c_start = 0
in_gap = False
cur_zeros = 0
cur_start = 0
window_size = 20
sum_max = 0
cur_sum = 0
mid_idx = 0
go_far = False
section_len = 100
# check if no gap big enough exists
if num_zeros < 150:
go_far = True
for idx, depth in enumerate(c_depths_2):
if idx - section_len < len(c_depths_2):
cur_sum = sum(c_depths_2[idx:idx + section_len - 1])
if cur_sum < sum_max:
sum_max = cur_sum
mid_idx = idx + int(section_len/2)
print("Go at the furthest object!")
no_pub = False
if go_far:
frac = mid_idx/float(len(c_depths))
frac = frac * 90.0
ang = -(frac - 45.0)
#ang = ang - (0.2 * ang)
if ang < 0:
if Clockwise:
ang = -45.0
else:
ang = 45.0
else:
if Clockwise:
ang = 45.0
else:
ang = -45.0
real_ang = ang
print('Angle', ang, 'start', start, 'num zeros', num_zeros)
msg = MotorCommand()
msg.joint_name = "drive"
msg.acceleration = 0
msg.speed = 1
if go_far:
msg.position = VELOCITY/2.0
else:
msg.position = VELOCITY
pololu_pub.publish(msg)
msg.joint_name = "steering"
if Clockwise:
ang = (((ang + 45.0)/90.0) * 2.0) - 1.0
else:
ang = (((ang + 45.0)/90.0) * 2.0) - 1.0 + 0.2
msg.acceleration = ACC
msg.speed = ACC
msg.position = ang
if not no_pub:
pololu_pub.publish(msg)
