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 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 __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 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 __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 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 __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 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 __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 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 __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 publish_steering_angle(angle):
cmd = MotorCommand()
print("Angle: " + str(angle))
ftg_msg = Float64()
ftg_msg.data = angle
pub.publish(ftg_msg)
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 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(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
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 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 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 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(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 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 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 __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 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)
#!/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)
#!/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:
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 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)
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 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
from std_msgs.msg import Float32MultiArray from ros_pololu_servo.msg import MotorCommand import numpy as np import time # Set constants MAX_SPEED = 0.3 MIN_SPEED = 0 MAX_ANGLE = 0.54 SERVO_ACCELERATION = 0.04 DRIVE_ACCELERATION = 0.01 TURN_ANGLE = 0.54 ## Configure servo command channel # Servo -0.54(left) to + 0.54(right) servo_commands = MotorCommand() servo_commands.joint_name = 'servo' servo_commands.position = 0 servo_commands.speed = 0 servo_commands.acceleration = SERVO_ACCELERATION ## Configure drive motor command channel # DC Motor Min 0.05 Max 0.45 (0.62 but we don't see a change) drive_commands = MotorCommand() drive_commands.joint_name = 'motor' drive_commands.position = 0 #Set speed here drive_commands.speed = 0 drive_commands.acceleration = DRIVE_ACCELERATION s_angle = 0