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 __init__(self, sub_topic_name = '/cmd_vel'):
# publisher
self._drive_publisher = rospy.Publisher('/pololu/command', MotorCommand, queue_size=1)
# subscriber
self._sub_topic_name = sub_topic_name
self._twist_subscriber = rospy.Subscriber( self._sub_topic_name, Twist_float, self.topic_callback)
# get message
self._drive_msg_motor = MotorCommand()
self._drive_msg_steering = MotorCommand()
self._twist_msg = Twist_float()
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 __init__(self):
self.timeVar = 5e9
#self.didIStop = False
#Publisher for sending commands to the pololu
self.motor_turning_pub = rospy.Publisher('pololu/command',
MotorCommand,
queue_size=1)
self.cmd_turning = MotorCommand()
self.cmd_turning.joint_name = 'front_turning'
self.cmd_turning.speed = 0
self.cmd_turning.acceleration = 0
#Publisher for sending commands to the pololu
self.motor_driving_pub = rospy.Publisher('pololu/command',
MotorCommand,
queue_size=1)
self.cmd_driving = MotorCommand()
self.cmd_driving.joint_name = 'back_motor'
self.cmd_driving.speed = 0
self.cmd_driving.acceleration = 0
#Subscriber for the PID control msgs
self.sub_turning_control = rospy.Subscriber(
'/turning_PID/control_effort',
Float64,
self.subCallback_Turning_Control,
queue_size=1)
#Subscriber for the PID control msgs
self.sub_driving_control = rospy.Subscriber(
'/driving_PID/control_effort',
Float64,
self.subFront_PID,
queue_size=1)
#Subscriber for the IMU control msgs
self.sub_driving_throttle = rospy.Subscriber(
'/throttle/effort',
Float64,
self.subCallback_Driving_Control,
queue_size=1)
#Subscruber for the Stop Sign Signal
self.sub_stop_sign = rospy.Subscriber('/stop_sign/bool_to_stop',
Bool,
self.subCallback_Stop_Sign,
queue_size=1)
#this block is to send the desired setpoint
self.setpoint_turning = 0
self.setpoint_driving = 100
self.front_previous = 0
self.stop = False
self.ratio_Max = .6
self.last = rospy.get_rostime().nsecs
self.stop = False
self.cmd_driving.position = .15
def __init__(self):
self.joint_sub = rospy.Subscriber('/joint_states', JointState,
self.joint_callback)
self.head_pan_relax = rospy.ServiceProxy('/head_pan_joint/relax',
Relax)
self.head_tilt_relax = rospy.ServiceProxy('/head_tilt_joint/relax',
Relax)
self.left_elbow_relax = rospy.ServiceProxy('/left_elbow/relax', Relax)
self.left_sho_pitch_relax = rospy.ServiceProxy('/left_sho_pitch/relax',
Relax)
self.left_sho_roll_relax = rospy.ServiceProxy('/left_sho_roll/relax',
Relax)
self.right_elbow_relax = rospy.ServiceProxy('/right_elbow/relax',
Relax)
self.right_sho_pitch_relax = rospy.ServiceProxy(
'/right_sho_pitch/relax', Relax)
self.right_sho_roll_relax = rospy.ServiceProxy('/right_sho_roll/relax',
Relax)
# Ref: https://github.com/geni-lab/hri_common/blob/master/scripts/mini_head_lip_sync_node.py
self.motor_pub = rospy.Publisher('pololu/command',
MotorCommand,
queue_size=1)
self.motor_range_srv = rospy.ServiceProxy('pololu/motor_range',
MotorRange)
# Setup motor command
self.left_sho_roll = MotorCommand()
self.left_sho_roll.joint_name = 'left_shoulder_roll_joint'
self.left_sho_roll.speed = 1.0
self.left_sho_roll.acceleration = 1.0
self.left_sho_pitch = MotorCommand()
self.left_sho_pitch.joint_name = 'left_shoulder_pitch_joint'
self.left_sho_pitch.speed = 1.0
self.left_sho_pitch.acceleration = 1.0
self.right_sho_roll = MotorCommand()
self.right_sho_roll.joint_name = 'right_shoulder_roll_joint'
self.right_sho_roll.speed = 1.0
self.right_sho_roll.acceleration = 1.0
self.right_sho_pitch = MotorCommand()
self.right_sho_pitch.joint_name = 'right_shoulder_pitch_joint'
self.right_sho_pitch.speed = 1.0
self.right_sho_pitch.acceleration = 1.0
threading.Thread.__init__(self)
self.sleeper = rospy.Rate(10)
def __init__(self):
self.timeVar = 5e9
#Publisher for sending commands to the pololu
self.motor_turning_pub = rospy.Publisher('pololu/command', MotorCommand, queue_size=1)
self.cmd_turning = MotorCommand()
self.cmd_turning.joint_name = 'front_turning'
self.cmd_turning.speed = 0
self.cmd_turning.acceleration = 0
#Publisher for sending commands to the pololu
self.motor_driving_pub = rospy.Publisher('pololu/command', MotorCommand, queue_size=1)
self.cmd_driving = MotorCommand()
self.cmd_driving.joint_name = 'back_motor'
self.cmd_driving.speed = 1
self.cmd_driving.acceleration = 1
#Publisher for sending the state_turning to the PID controller
self.state_turning_pub = rospy.Publisher('/turning_PID/state', Float64, queue_size=1)
self.state_turning = Float64()
#Publisher for sending a setpoint_turning to the PID controller
self.setpoint_turning_pub = rospy.Publisher('/turning_PID/setpoint', Float64, queue_size=1)
self.setpoint_turning = Float64()
#Publisher for sending the state_turning to the PID controller
self.state_driving_pub = rospy.Publisher('/driving_PID/state', Float64, queue_size=1)
self.state_driving = Float64()
#Publisher for sending a setpoint_turning to the PID controller
self.setpoint_driving_pub = rospy.Publisher('/driving_PID/setpoint', Float64, queue_size=1)
self.setpoint_driving = Float64()
#Subscriber for the state of the pololu motors
self.sub_pololu = rospy.Subscriber('/pololu/motor_states', MotorStateList, self.subCallback_Turning, queue_size=1)
#Subscriber for the PID control msgs
self.sub_turning_control = rospy.Subscriber('/turning_PID/control_effort', Float64, self.subCallback_Turning_Control, queue_size=1)
#Subscriber for the PID control msgs
self.sub_driving_control = rospy.Subscriber('/driving_PID/control_effort', Float64, self.subCallback_Driving_Control, queue_size=1)
self.sub_imu = rospy.Subscriber('/imu/data_raw', Imu, self.imuCallBack, queue_size=1)
#this block is to send the desired setpoint
self.setpoint_turning = 130
self.setpoint_driving = 120
self.front_previous = 0
self.ratio_Max = 1
self.last = rospy.get_rostime().nsecs
self.stop = False
self.input = []
self.output = []
self.ALPHA = 0.15
def __init__(self):
self.motor_pub = rospy.Publisher('pololu/command',
MotorCommand,
queue_size=1)
self.motor_range_srv = rospy.ServiceProxy('pololu/motor_range',
MotorRange)
# Setup motor command
self.cmd = MotorCommand()
self.cmd.joint_name = 'jaw_joint'
self.cmd.speed = 1.0
self.cmd.acceleration = 1.0
# Add some comment here.
# Get motor range
rospy.loginfo('Waiting for pololu/motor_range service...')
self.motor_range_srv.wait_for_service()
rospy.loginfo('Found')
response = self.motor_range_srv(self.cmd.joint_name)
self.max_range = max([response.min, response.max])
self.sub = rospy.Subscriber('speech_strength',
Float32,
self.speech_strength_callback,
queue_size=1)
def __init__(self, joint_name):
Joint.__init__(self, joint_name)
self.msg = MotorCommand()
self.msg.joint_name = self.joint_name
self.motor_pub = rospy.Publisher('pololu/command',
MotorCommand,
queue_size=10)
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 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 publish_steering_angle(angle):
cmd = MotorCommand()
print("Angle: " + str(angle))
ftg_msg = Float64()
ftg_msg.data = angle
pub.publish(ftg_msg)
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):
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 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 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)
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 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 __init__(self):
self.motor_pub = rospy.Publisher('pololu/command',MotorCommand, queue_size=1)
self.motor_range_srv = rospy.ServiceProxy('pololu/motor_range',MotorRange)
# set up motor command
self.cmd = MotorCommand()
self.cmd.joint_name = 'ext_joint'
# set motor speed
self.cmd.speed = .7
self.cmd.acceleration = 0.0
# get motor range
rospy.loginfo('Waiting for pololu/motor_range service...')
self.motor_range_srv.wait_for_service()
rospy.loginfo('Service found')
response = self.motor_range_srv(self.cmd.joint_name)
self.max_range = max([respone.min, response.max])
self.sub = rospy.Subscriber('ext_command',Float32, self.ext_callback, queue_size=1)
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 __init__(self):
self.motor_topic = '/pololu/command'
self.front_topic = '/front_controller/command'
self.back_topic = '/back_controller/command'
self.drive_topic = '/drive/command'
self.cmd_motor = MotorCommand()
self.cmd_front = Float64()
self.cmd_back = Float64()
#create publisher passing it the vel_topic_name and msg_type
self.motor_pub = rospy.Publisher(self.motor_topic,
MotorCommand,
queue_size=1)
self.front_pub = rospy.Publisher(self.front_topic,
Float64,
queue_size=1)
self.back_pub = rospy.Publisher(self.back_topic, Float64, queue_size=1)
#create subscriber
self.drive_sub = rospy.Subscriber(self.drive_topic, DriveCommand,
self.scan_callback)
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)
import sys, signal
def signal_handler(signal, frame):
print("\nprogram exiting gracefully")
sys.exit(0)
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"
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))
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()
#!/usr/bin/env python
import rospy
from std_msgs.msg import String
from geometry_msgs.msg import PoseStamped, Point, Twist
from ros_pololu_servo.msg import MotorCommand
from ros_pololu_servo.msg import HARECommand
steering_cmd_msg = MotorCommand()
motor_cmd_msg = MotorCommand()
def cmd_cb(msg):
# Limit incoming message to steering limits
if msg.steering_angle > .78:
msg.steering_angle = .78
elif msg.steering_angle < -.78:
msg.steering_angle = -.78
steering_cmd_msg.position = msg.steering_angle
# Limit the incoming throttle command
if msg.throttle_cmd > 1:
msg.throttle_cmd = 1
elif msg.throttle_cmd < 0:
msg.throttle_cmd = 0.0
# Handle throttle mode
if msg.throttle_mode == 0:
motor_cmd_msg.position = 0.0
elif msg.throttle_mode == 3:
#!/usr/bin/env python
#Revision V1 11-19-19
#Updated with PID controller inital test
#
#
#
#
#
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)
