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 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 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 callback(data):
servo_cmd = MotorCommand()
servo_cmd.joint_name = 'Servo'
servo_cmd.position = (data.data * .007)
pub_MotorCmd.publish(servo_cmd)
motor_command = MotorCommand()
motor_command.joint_name = "ESC"
motor_command.position = .7
pub_MotorCmd.publish(motor_command)
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 callback(data):
control_effort = data.data
servo_cmd = MotorCommand()
if (control_effort < -20):
servo_cmd.position = float(-.6)
elif (control_effort > 20):
servo_cmd.position = float(.6)
else:
servo_cmd.position = control_effort / 33.0
servo_cmd.joint_name = "Servo"
pub_servo.publish(servo_cmd)
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 steer_pub(self):
servo_cmd = MotorCommand()
#print self.actuator_states.motor_states[1].pulse
servo_cmd.position = float(
1 * (140 - self.actuator_states.motor_states[1].pulse)) / 215
servo_cmd.joint_name = "steering"
self.steering_pub.publish(servo_cmd)
def callback(data):
diff = data.motor_states[1].pulse - data.motor_states[2].pulse
print diff
servo_cmd = MotorCommand()
if (diff < -100):
servo_cmd.position = float(-0.6)
elif (diff > 100):
servo_cmd.position = 0.6
else:
servo_cmd.position = diff / 167.0
servo_cmd.position = -servo_cmd.position
servo_cmd.joint_name = "Servo"
pub.publish(servo_cmd)
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 = 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)
def callback(data):
control_effort = data.data
esc_cmd = MotorCommand()
if(control_effort < 0):
esc_cmd.position = .305
elif(control_effort >= 60):
esc_cmd.position = .345
else:
esc_cmd.position = .305 + control_effort/1200.0
esc_cmd.joint_name = 'ESC'
pub_MotorCmd.publish(esc_cmd)
'''comment me out to return steering controll'''
servo_cmd = MotorCommand()
servo_cmd.joint_name = 'Servo'
servo_cmd.position = 0.0
pub_MotorCmd.publish(servo_cmd)
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 callback_set_state(self, data):
print "set_state_callback"
current_bemf = data.data #motor angle
speed_cmd = MotorCommand()
speed_cmd.joint_name = "ESC"
if (current_bemf <= self.set_state):
motor_angle = .35
elif (current_bemf > self.set_state):
motor_angle = .2
speed_cmd.position = motor_angle
pub_esc.publish(speed_cmd)
servo_cmd.position = (data.data * .007)
pub_MotorCmd.publish(servo_cmd)
motor_command = MotorCommand()
motor_command.joint_name = "ESC"
motor_command.position = .7
pub_MotorCmd.publish(motor_command)
if __name__ == '__main__':
print "hello I am the servo controller"
rospy.init_node("servo_control_pub", anonymous = False)
pub_MotorCmd = rospy.Publisher("/pololu/command", MotorCommand, queue_size = 1)
motor_setup_delay = rospy.Rate(.1)
print "starting motor zero point"
motor_package = MotorCommand()
motor_package.joint_name = "ESC"
motor_package.position = .5
pub_MotorCmd.publish(motor_package)
motor_setup_delay.sleep()
print "motor hold finished"
rospy.Subscriber("/servo_control_effort", Float64, callback)
rospy.spin()
def callback(data):
pub = rospy.Publisher('motor_command', MotorCommand, queue_size=1)
msg = MotorCommand()
bridge = CvBridge()
depth = bridge.imgmsg_to_cv2(data, data.encoding)
height = data.height
width = data.width
depth_image_cut = depth[depth_upper_bound:depth_lower_bound, :]
median_depth = np.transpose(np.nanmedian(depth_image_cut, axis=0))
obstacles = get_obstacles(median_depth, depth_thresh)
pruned_obstacles, pruned_median = get_pruned_obstacles(
obstacles, median_depth)
largest_gap, gap_depth = find_largest_gap(pruned_obstacles, pruned_median)
car_pos = [340, 0]
left_obs = [largest_gap[0], gap_depth[0]]
right_obs = [largest_gap[1], gap_depth[1]]
l_neg = False
r_neg = False
if largest_gap[0] > 340:
l_neg = True
if largest_gap[1] > 340:
r_neg = True
vec_left_obs = [largest_gap[0] - car_pos[0], gap_depth[0]]
vec_right_obs = [largest_gap[1] - car_pos[0], gap_depth[1]]
# Get left, right and middle line lengths
left_len = math.sqrt((car_pos[0] - left_obs[0])**2 +
(car_pos[1] - left_obs[1])**2)
right_len = math.sqrt((car_pos[0] - right_obs[0])**2 +
(car_pos[1] - right_obs[1])**2)
middle_len = math.sqrt((right_obs[0] - left_obs[0])**2 +
(right_obs[1] - left_obs[1])**2)
# Calculate left and right angles
vec_center = [0, 1]
center_len = 1
left_dot_prod = vec_left_obs[0] * vec_center[0] + vec_left_obs[
1] * vec_center[1]
right_dot_prod = vec_right_obs[0] * vec_center[0] + vec_right_obs[
1] * vec_center[1]
left_cos_theta = float(left_dot_prod) / (left_len * center_len)
right_cos_theta = float(right_dot_prod) / (right_len * center_len)
rad_left = math.acos(left_cos_theta)
rad_right = math.acos(right_cos_theta)
ang_left = math.degrees(rad_left)
if l_neg:
ang_left *= -1
print('left angle: ' + str(ang_left))
ang_right = math.degrees(rad_right)
if r_neg:
ang_right *= -1
print('right angle: ' + str(ang_right))
# Simpler method of calculating the depth angle
avg_ang = (ang_left + ang_right) / 2
global num_samples
global sample_measurements
if num_samples < 50:
sample_measurements[avg_ang] = gap_depth
num_samples += 1
# Calculate the gap angle (precise method)
# cos_added = math.cos(rad_left + rad_right)
# numer = left_len + right_len * cos_added
# denom = math.sqrt(left_len**2 + right_len**2 + 2*left_len*right_len*cos_added)
# rad_gap = math.acos(numer/denom) - rad_left
# ang_gap = math.degrees(rad_gap)
pololu_pub = rospy.Publisher('pololu/command', MotorCommand, queue_size=10)
# print('depth', type(depth))
# print("height", height, 'width', width)
msg.joint_name = 'steering'
#ang_mapped = math.radians(-avg_ang)
ang_mapped = (((avg_ang + 30) / 60) * 1.4) - 0.7
msg.position = ang_mapped
pololu_pub.publish(msg)
print('angle output: ' + str(avg_ang))
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)
if __name__ == '__main__':
signal.signal(signal.SIGINT, signal_handler)
init_node('forward_node')
pub_esc = rospy.Publisher("/pololu/command", MotorCommand, queue_size=1)
speed_cmd = MotorCommand()
speed_cmd.joint_name = "ESC"
current_speed = .33
while not is_shutdown():
print "faster or slower"
command = raw_input()
print command + "received"
if (command == "w"):
current_speed += .02
elif (command == "s"):
current_speed -= .02
else:
print "shit command"
print "current speed: " + str(current_speed)
speed_cmd.position = current_speed
pub_esc.publish(speed_cmd)
#
#
#
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
