Beispiel #1
0
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   
Beispiel #2
0
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)
Beispiel #5
0
    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)
Beispiel #9
0
 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)
Beispiel #10
0
 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)
Beispiel #11
0
 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)
Beispiel #12
0
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)
Beispiel #14
0
    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)
Beispiel #17
0
    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)
Beispiel #19
0
    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)
Beispiel #20
0
	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)
Beispiel #21
0
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)
Beispiel #22
0
    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)
Beispiel #23
0
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)
Beispiel #24
0
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"
Beispiel #25
0
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()
Beispiel #27
0
#!/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)