def test1():
    print("test 1")
    global x1, y1, position1, position2, endTicks
    rospy.sleep(3)
    position1 = [x1, y1]
    print("start position: " + str(position1))
    steering_command = NormalizedSteeringCommand()
    steering_command.value = 1.0
    publish_steering.publish(steering_command)
    rospy.sleep(1)
    speed_command = SpeedCommand()
    speed_command.value = 0.2
    publish_speed.publish(speed_command)
    rospy.sleep(5)
    speed_command.value = 0.0
    publish_speed.publish(speed_command)
    position2 = [x1, y1]
    print("end position: " + str(position2))
    print("total ticks: " + str(endTicks))
    euclideanDistance = euclidean_distance(position1[0], position1[1],
                                           position2[0], position2[1])
    current_slope = calculate_slope(position1[0], position1[1], position2[0],
                                    position2[1])
    current_angle = math.atan(current_slope)
    print("current angle: " + str(current_angle))
    radius = (euclideanDistance / (2 * math.sin(current_angle / 2)))
    print("radius: " + str(radius))
    distanceDriven = radius * current_angle
    print("total distance driven: " + str(distanceDriven))
    ratio = distanceDriven / endTicks
    print("ratio between travelled distance and counted ticks: " + str(ratio))
def main():
    while curX == 0.0:
        pass
    pos1 = (curX,curY)
    time.sleep(1)
    steering_cmd = NormalizedSteeringCommand()
    steering_cmd.value = 1.0
    pub_steering.publish(steering_cmd)
    time.sleep(1)
    speed_cmd = SpeedCommand()
    speed_cmd.value = 0.2
    pub_speed.publish(speed_cmd)
    time.sleep(4)
    speed_cmd.value = 0.0
    pub_speed.publish(speed_cmd)
    time.sleep(2)
    pos2 = (curX,curY)
    speed_cmd.value = 0.2
    pub_speed.publish(speed_cmd)
    time.sleep(4)
    speed_cmd.value = 0.0
    pub_speed.publish(speed_cmd)
    time.sleep(2)
    pos3 = (curX,curY)
    print "Radius:",berechneRadius(pos1,pos2,pos3),"Meter"
    print "Winkel:",berechneEinlenkWinkel(berechneRadius(pos1,pos2,pos3)),"Grad, weil niemand was mit Bogenmass anfangen kann"
Esempio n. 3
0
def timer():
    h = Header()
    h.stamp = rospy.Time.now()
    mensaje = NormalizedSteeringCommand()
    mensaje.header = h
    mensaje.value = -0.45
    pub.publish(mensaje)
Esempio n. 4
0
def driveForwardDistance():
    global positions, accTickDistance, gpsDistance
    i = 1

    steering_cmd = NormalizedSteeringCommand()
    steering_cmd.value = 0.0  #left 1.0 , right -1.0
    pub_steering.publish(steering_cmd)
    speed = SpeedCommand()
    speed.value = 0.2  #moeglichst langsam
    pub_speed.publish(speed)
    time.sleep(4)

    while i < len(positions):
        gpsDistance += math.sqrt(
            math.pow(positions[i][0] - positions[i - 1][0], 2) +
            math.pow(positions[i][1] - positions[i - 1][1], 2))
        accTickDistance += accTick

        # print(gpsDistance)
        i = i + 1

    rospy.loginfo("cumulativ gpsDistance (cm): " + str(gpsDistance))
    rospy.loginfo("last - first position gps distance (cm) : " + str(
        math.sqrt(
            math.pow(positions[len(positions) - 1][0] - positions[0][0], 2) +
            math.pow(positions[len(positions) - 1][1] - positions[0][1], 2))))
    rospy.loginfo("Ticks: " + str(accTickDistance))
    rospy.loginfo("Distance per Tick (cumulativ gpsDistance (cm)/ ticks): " +
                  str(gpsDistance / accTickDistance))
    speed.value = 0.0
    pub_speed.publish(speed)
Esempio n. 5
0
    def PID(self):
        Kp = 5
        Ki = 0
        Kd = 0.1
        error = (self.setpoint - self.yaw)

        self.acc_error = self.acc_error + error  #accumulator for error
        current_time = rospy.Time.now()
        delta_time = (current_time - self.pre_time).to_sec()
        #Kp*error Ki*area Kd*slope

        PID = Kp * error + Ki * self.acc_error * delta_time + Kd * (
            error - self.pre_error) / delta_time
        # PID = int(round(PID))
        self.pre_time = current_time
        self.pre_error = error

        #pid value to steering message
        str_com = NormalizedSteeringCommand()
        str_com.value = PID
        self.str_pub.publish(str_com)

        #pub speed
        sp = SpeedCommand()
        sp.value = 0.3
        self.speed_pub.publish(sp)
Esempio n. 6
0
def publisher():

    pub_steer = rospy.Publisher('/actuators/steering_normalized',
                                NormalizedSteeringCommand,
                                queue_size=10)
    pub_speed = rospy.Publisher('/actuators/speed',
                                SpeedCommand,
                                queue_size=10)
    rospy.init_node('publisher', anonymous=True)
    rate = rospy.Rate(10)

    while not rospy.is_shutdown():
        '''
        rospy.set_param('msg_steer', 1.0)
	msg_steer = rospy.get_param("msg_steer")
        rospy.loginfo(msg_steer)
        pub_steer.publish(msg_steer)

        rospy.set_param('msg_speed' , 0.3)
	msg_speed = rospy.get_param("msg_speed")
        rospy.loginfo(msg_speed)
        pub_speed.publish(msg_speed)

        rate.sleep()
	'''
        msg_steer = NormalizedSteeringCommand()
        msg_steer.value = 1.0
        rospy.loginfo(msg_steer)
        pub_steer.publish(msg_steer)
        msg_speed = SpeedCommand()
        msg_speed.value = 0.3
        rospy.loginfo(msg_speed)
        pub_speed.publish(msg_speed)
Esempio n. 7
0
    def on_control(self, tmr):

        if tmr.last_duration is None:
            dt = 0.01
        else:
            dt = (tmr.current_expected - tmr.last_expected).to_sec()

        # print dt

        quat = [
            self.pose.pose.pose.orientation.x,
            self.pose.pose.pose.orientation.y,
            self.pose.pose.pose.orientation.z,
            self.pose.pose.pose.orientation.w
        ]
        roll, pitch, yaw = tf.transformations.euler_from_quaternion(quat)
        error = self.desired_angle

        self.integral_error += error * dt
        self.integral_error = max(self.min_i, self.integral_error)
        self.integral_error = min(self.max_i, self.integral_error)

        derivative_error = (error - self.last_error) / dt
        self.last_error = error

        pid_output = self.kp * error + self.kd * derivative_error + self.ki * self.integral_error

        steering_msg = NormalizedSteeringCommand()
        steering_msg.value = pid_output
        self.steering_pub.publish(steering_msg)
Esempio n. 8
0
def drive(distance, command, speed, angle):
    global is_active
    speed = speed * speed_mult
    rospy.loginfo("speed is %f", speed)

    rospy.loginfo("%s: Running %s(%f)", rospy.get_caller_id(), command, distance)
    if distance <= 0:
        rospy.logerr(
            "%s: Error, distance argument has to be > 0! %f given",
            rospy.get_caller_id(),
            distance)
        return

    pub_info.publish("BUSY")
    if is_active:
        rospy.logwarn(
            "%s: Warning, another command is still active! Please wait and try again.",
            rospy.get_caller_id())
        return

    is_active = True

    # stop the car and set desired steering angle + speed
    speed_cmd = SpeedCommand()
    speed_cmd.value = 0
    pub_speed.publish(speed_cmd)
    rospy.sleep(1)
    steering_cmd = NormalizedSteeringCommand()
    steering_cmd.value = angle
    pub_steering.publish(steering_cmd)
    rospy.sleep(1)
    speed_cmd.value = speed
    pub_speed.publish(speed_cmd)

    start_pos = last_odom.pose.pose.position
    current_distance = 0

    while not rospy.is_shutdown() and current_distance < (distance - epsilon):
        current_pos = last_odom.pose.pose.position
        current_distance = sqrt(
            (current_pos.x - start_pos.x)**2 + (current_pos.y - start_pos.y)**2)
        # rospy.loginfo("current distance = %f", current_distance)
        rospy.sleep(0.1)

    speed_cmd.value = 0
    pub_speed.publish(speed_cmd)
    is_active = False
    current_pos = last_odom.pose.pose.position
    current_distance = sqrt((current_pos.x - start_pos.x)
                            ** 2 + (current_pos.y - start_pos.y)**2)
    pub_info.publish("FINISHED")

    rospy.loginfo(
        "%s: Finished %s(%f)\nActual travelled distance = %f",
        rospy.get_caller_id(),
        command,
        distance,
        current_distance)
Esempio n. 9
0
def driver(steer_pub, speed_pub):
    steer_msg = NormalizedSteeringCommand()
    steer_msg.value = -1.0
    steer_pub.publish(steer_msg)
    speed_msg = SpeedCommand()
    speed_msg.value = 0.3
    speed_pub.publish(speed_msg)
    rospy.sleep(2.5)
    speed_msg.value = 0.0
    speed_pub.publish(speed_msg)
Esempio n. 10
0
def talker():
    pub = rospy.Publisher('/actuators/speed', SpeedCommand, queue_size=10)
    pub2 = rospy.Publisher('/actuators/steering_normalized', NormalizedSteeringCommand, queue_size=10)
    rospy.init_node('talker', anonymous=True)
    rate = rospy.Rate(10) # 10hz
    while not rospy.is_shutdown():
        msg = SpeedCommand()
        msg.value = 0.5
        pub.publish(msg)
        msg2 = NormalizedSteeringCommand()
        msg2.value = 1.0
        pub2.publish(msg2)
        rate.sleep()
Esempio n. 11
0
def talker():
    steer_pub = rospy.Publisher('/actuators/steering_normalized', NormalizedSteeringCommand, queue_size=10)
    speed_pub = rospy.Publisher('/actuators/speed', SpeedCommand, queue_size=10)
    rospy.init_node('publisher', anonymous=True)
    rate = rospy.Rate(10) # 10hz
    while not rospy.is_shutdown():
        steer_msg = NormalizedSteeringCommand()
        steer_msg.value = 1.0
        steer_pub.publish(steer_msg)
        speed_msg = SpeedCommand()
        speed_msg.value = 0.6
        speed_pub.publish(speed_msg)
        rate.sleep()
Esempio n. 12
0
 def on_localization(self, data):
     current_pos = np.array(
         [data.pose.pose.position.x, data.pose.pose.position.y])
     look_ahead_point = self.map.lanes[self.lane].lookahead_point(
         current_pos, 0.5)[0]  # [0] for lookahead point  inner lane
     self.publish_looakhead(look_ahead_point)
     car_yaw = self.get_car_yaw(data)
     yaw = self.calculateYaw(current_pos, look_ahead_point,
                             car_yaw)  # yaw difference from two points
     print("I want this yaw:" + str(yaw))
     steering_msg = NormalizedSteeringCommand()
     steering_msg.value = yaw
     self.pid_desired_angle.publish(steering_msg)
Esempio n. 13
0
    def callback(self, raw_msgs):
        position = raw_msgs.pose.pose.position
        x = position.x
        y = position.y
        current_point = [x, y]
        # math transformation
        orientation = raw_msgs.pose.pose.orientation
        quaternion = [
            orientation.x, orientation.y, orientation.z, orientation.w
        ]
        euler = tf.transformations.euler_from_quaternion(quaternion)
        self.yaw = euler[2]

        def rot(yaw):
            return np.array([[math.cos(yaw), -math.sin(yaw)],
                             [math.sin(yaw), math.cos(yaw)]])

        map = myMap.Map()
        lane = map.lanes[self.lane]
        goal_point = lane.lookahead_point(current_point, 0.5)[0]
        vector = goal_point - current_point
        map_point = np.matmul(rot(-self.yaw), vector)
        self.setpoint = np.arctan2(map_point[1], map_point[0])
        #  np.arccos(np.dot(current_point,goal_point)/(np.linalg.norm(current_point)*np.linalg.norm(goal_point)))

        Kp = 2.0
        Ki = 0.0
        Kd = 0.2
        error = self.setpoint  #- self.yaw

        self.acc_error = self.acc_error + error  #accumulator for error
        current_time = rospy.Time.now()
        delta_time = (current_time - self.pre_time).to_sec()
        #Kp*error Ki*area Kd*slope

        PID = Kp * error + Ki * self.acc_error * delta_time + Kd * (
            error - self.pre_error) / delta_time
        # PID = int(round(PID))
        self.pre_time = current_time
        self.pre_error = error

        #pid value to steering message
        str_com = NormalizedSteeringCommand()
        str_com.value = PID
        self.str_pub.publish(str_com)

        #pub speed
        sp = SpeedCommand()
        sp.value = 0.3
        self.speed_pub.publish(sp)
Esempio n. 14
0
def driver():
    steer_pub = rospy.Publisher('/actuators/steering_normalized',
                                NormalizedSteeringCommand,
                                queue_size=10)
    speed_pub = rospy.Publisher('/actuators/speed',
                                SpeedCommand,
                                queue_size=10)
    rate = rospy.Rate(10)  # 10hz
    while not rospy.is_shutdown():
        steer_msg = NormalizedSteeringCommand()
        steer_msg.value = 1.0
        steer_pub.publish(steer_msg)
        speed_msg = SpeedCommand()
        speed_msg.value = 0.3
        speed_pub.publish(speed_msg)
Esempio n. 15
0
def comienzo():
    rospy.init_node('parking2', anonymous=True)
    pub = rospy.Publisher('/control/command/normalized_wanted_speed', NormalizedSpeedCommand, queue_size=10)
    steering = rospy.Publisher('/control/command/normalized_wanted_steering', NormalizedSteeringCommand, queue_size=10)

    h = std_msgs.msg.Header()
    h.stamp = rospy.Time.now()
    mensaje = mensaje2.value()
    mensaje.header = h
    mensaje.value  = date.value

    j = std_msgs.msg.Header()
    j.stamp = rospy.Time.now()
    mensaje2 = NormalizedSteeringCommand()
    mensaje2.header = j
    mensaje2.value = 0.5
Esempio n. 16
0
def driver():
    steer_pub = rospy.Publisher('/actuators/steering_normalized',
                                NormalizedSteeringCommand,
                                queue_size=10)
    speed_pub = rospy.Publisher('/actuators/speed',
                                SpeedCommand,
                                queue_size=10)
    steer_msg = NormalizedSteeringCommand()
    steer_msg.value = 1.0
    steer_pub.publish(steer_msg)
    speed_msg = SpeedCommand()
    speed_msg.value = 0.3
    speed_pub.publish(speed_msg)
    rospy.sleep(1)
    speed_msg = SpeedCommand()
    speed_msg.value = 0.0
    speed_pub.publish(speed_msg)
    def callbackGPS(self, gps_data):
        # calculate how off steering is and give the error to PID Controler
        orientation = gps_data.pose.pose.orientation
        # print ("gps: " , gps_data)

        current_yaw = self.get_rotation(orientation)
        error = self.desired_angle - current_yaw
        if (abs(error) > self.accepted_error):
            update = self.PID_controller.get_PID(error)
            print("err:", error)
            print("yaw: ", current_yaw)
            print("UPD: ", update)
            steering = NormalizedSteeringCommand()
            steering.value = update
            self.pub_steering.publish(steering)
            self.pub_speed.publish(self.speed_cmd)
            rate.sleep()
Esempio n. 18
0
    def __init__(self):
        rospy.init_node("basic_publisher")
        self.speed_publisher = rospy.Publisher("/actuators/speed", SpeedCommand, queue_size=1)
        self.steering_publisher = rospy.Publisher("/actuators/steering_normalized", NormalizedSteeringCommand,
                                                  queue_size=1)

        self.r = rospy.Rate(10)  # 10hz

        while not rospy.is_shutdown():
            steer_msg = NormalizedSteeringCommand()
            steer_msg.value = 1.0
            self.steering_publisher.publish(steer_msg)

            speed_msg = SpeedCommand()
            speed_msg.value = 0.3
            self.speed_publisher.publish(speed_msg)

            self.r.sleep()
Esempio n. 19
0
 def __init__(self):
     print("initializing Driver")
     rospy.init_node("assignment_6", anonymous = True)
     self.rate = rospy.Rate(10)
     self.steer_pub = rospy.Publisher("/actuators/steering_normalized", NormalizedSteeringCommand, queue_size=10)
     self.speed_pub = rospy.Publisher("/actuators/speed", SpeedCommand, queue_size=10)
     self.steer_cmd = NormalizedSteeringCommand()
     self.speed_cmd = SpeedCommand()
     self.speed_sub = rospy.Subscriber("/actuators/speed", Speed, self.print_speed_given, queue_size=10)
     print("initialized complete")
Esempio n. 20
0
def backwards_right(data, date):
    rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.value)
    pub = rospy.Publisher('/control/command/normalized_wanted_speed', NormalizedSpeedCommand, queue_size=10)
    steering = rospy.Publisher('/control/command/normalized_wanted_steering', NormalizedSteeringCommand, queue_size=10)
    h = std_msgs.msg.Header()
    h.stamp = rospy.Time.now()
    mensaje = NormalizedSpeedCommand()
    mensaje.header = h
    mensaje.value  = data.value

    j = std_msgs.msg.Header()
    j.stamp = rospy.Time.now()
    mensaje2 = NormalizedSteeringCommand()
    mensaje2.header = j
    mensaje2.value = date.value

    if date.value > 0:
       mensaje.vlaue = date.value * -1
    pub.publish(mensaje)
Esempio n. 21
0
  def callback(self,raw_msgs):
    #print("call")
    msg=raw_msgs.pose.pose.orientation
    msg_array=[msg.x,msg.y,msg.z,msg.w]
    #print(str(msg))
    euler=tf.transformations.euler_from_quaternion(msg_array)
    #print(euler)
    steering=NormalizedSteeringCommand()
    steering.value=self.pid(euler[2])
    speed=SpeedCommand()
    speed.value=0.5
    #print(str(steering.value))
    #cv2.imshow('image',img)
    #cv2.waitKey(3)
    self.x4.publish(speed)

    try:
      self.x3.publish(steering)
    except CvBridgeError as e:
      print(e)
Esempio n. 22
0
def publish():
    pub_speed = rospy.Publisher('/actuators/speed',
                                SpeedCommand,
                                queue_size=10)
    pub_steering = rospy.Publisher("/actuators/steering_normalized",
                                   NormalizedSteeringCommand,
                                   queue_size=10)

    steering_msg = NormalizedSteeringCommand()
    steering_msg.value = 1
    speed_msg = SpeedCommand()
    speed_msg.value = 0.3

    rospy.init_node('publisher', anonymous=True)
    rate = rospy.Rate(10)  # 10hz
    while not rospy.is_shutdown():
        pub_steering.publish(steering_msg)
        pub_speed.publish(speed_msg)
        rospy.loginfo('speed is ' + str(speed_msg.value))
        print('speed is ' + str(speed_msg.value))
        rate.sleep()
def talker():
    # Publish to this existing topics
    pub_steering = rospy.Publisher('/actuators/steering_normalized', NormalizedSteeringCommand, queue_size=10)
    pub_speed = rospy.Publisher('/actuators/speed', SpeedCommand, queue_size=10)

    rospy.init_node('steering_speed_whisperer', anonymous=True)

    # steering
    norm_steerin_cmd = NormalizedSteeringCommand()  # Command is a class create object
    norm_steerin_cmd.value = 0


    #speed init
    speed_cmd = SpeedCommand()
    speed_cmd.value = 0.3
    rate = rospy.Rate(5) # 5 hz
    while not rospy.is_shutdown():
        info_str = "time {time} : speed: {speed} , steering: {steering}".format(time=rospy.get_time(),speed=speed_cmd.value,steering=norm_steerin_cmd.value)
        rospy.loginfo(info_str)
        pub_steering.publish(norm_steerin_cmd)
        pub_speed.publish(speed_cmd)
        rate.sleep()
Esempio n. 24
0
    def on_control(self, tmr):

        if tmr.last_duration is None:
            dt = 0.01
        else:
            dt = (tmr.current_expected - tmr.last_expected).to_sec()

        if self.desired_angle is None:
            return

        quat = [
            self.pose.pose.pose.orientation.x,
            self.pose.pose.pose.orientation.y,
            self.pose.pose.pose.orientation.z,
            self.pose.pose.pose.orientation.w
        ]
        roll, pitch, yaw = tf.transformations.euler_from_quaternion(quat)

        diff = self.desired_angle - yaw
        # normalize steering angles (keep between -pi and pi)
        error = math.atan2(math.sin(diff), math.cos(diff))

        self.integral_error += error * dt
        self.integral_error = max(self.min_i, self.integral_error)
        self.integral_error = min(self.max_i, self.integral_error)

        derivative_error = (error - self.last_error) / dt
        self.last_error = error

        pid_output = self.kp * error + self.kd * derivative_error + self.ki * self.integral_error

        steering_msg = NormalizedSteeringCommand()
        steering_msg.value = pid_output
        steering_msg.header.frame_id = "base_link"
        steering_msg.header.stamp = rospy.Time.now()

        self.steering_pub.publish(steering_msg)
Esempio n. 25
0
    def __init__(self):
        rospy.init_node("initNull")
        self.speed = SpeedCommand()
        self.steer = NormalizedSteeringCommand()
        self.position = Odometry()
        self.speed_pub = rospy.Publisher("/actuators/speed", SpeedCommand, queue_size=10)
        self.steer_pub = rospy.Publisher("/actuators/steering_normalized", NormalizedSteeringCommand, queue_size=10)
        self.position_sub = rospy.Subscriber("/communication/gps/9", Odometry, self.on_position, queue_size=10)

        self.rate = rospy.Rate(100)

        while not rospy.is_shutdown():
            self.speed.value = 0.0
            self.speed_pub.publish(self.speed)

            self.steer.value = 0.0
            self.steer_pub.publish(self.steer)
            self.rate.sleep()
Esempio n. 26
0
def callback(data):    

    rospy.loginfo(rospy.get_caller_id() + "I heard %f", data.value)
    h = Header()
    h.stamp = Rospy.Time.now()
    mensaje = NormalizedSteeringCommand()
    mensaje.header = h
    mensaje.value = data.value

    if data.value < 0:
	mensaje.value = data.value * -1
    if data.value > 0:
        mensaje.value = data.value * -1
    
    pub.publish(mensaje)
Esempio n. 27
0
    def __init__(self):
        rospy.init_node("initSpeed", anonymous=True)
        self.speed = SpeedCommand()
        self.steer = NormalizedSteeringCommand()
        self.position = Odometry()
        self.speed_pub = rospy.Publisher("/actuators/speed",
                                         SpeedCommand,
                                         queue_size=10)
        self.steer_pub = rospy.Publisher("/actuators/steering_normalized",
                                         NormalizedSteeringCommand,
                                         queue_size=10)
        self.position_sub = rospy.Subscriber("/communication/gps/9",
                                             Odometry,
                                             self.on_position,
                                             queue_size=10)
        self.error_counter = 0
        self.rate = rospy.Rate(10)

        self.Kp = 0.0
        self.Ki = 0.0
        self.Kd = 0.0
        self.current_time = time.time()
        self.last_time = self.current_time
        self.error_time = 0.0
        self.error = 0.0
        self.P = 0.0
        self.I = 0.0
        self.D = 0.0
        self.PID = 0.0

        self.max_integrator = 5.0
        self.min_integrator = -5.0

        self.speed_value = 0.5
        self.steering_value = 0.0

        while not rospy.is_shutdown():
            self.drive()
            pass

        rospy.spin()
Esempio n. 28
0
    def __init__(self):
        self.map = Map()
        rospy.init_node("map_visualization")

        self.steering = SteeringCommand()
        self.steering_normalized = NormalizedSteeringCommand()
        self.position = Odometry()

        self.lane_pub = rospy.Publisher("lane", Marker, queue_size=10)
        self.position_sub = rospy.Subscriber("/sensors/localization/filtered_map", Odometry, self.on_position,
                                             queue_size=1)
        self.pid_pub = rospy.Publisher("/control/steering", SteeringCommand, queue_size=1)
        self.lane_switch = rospy.Subscriber("/control/lane_switch", UInt8, self.lane_switch, queue_size=1)
        self.current_line = self.map.lanes[0]
        self.rate = rospy.Rate(1)

        while not rospy.is_shutdown():

            i = 0
            for lane in self.map.lanes:
                msg = Marker(type=Marker.LINE_STRIP, action=Marker.ADD)
                msg.header.frame_id = "map"
                msg.scale.x = 0.01
                msg.scale.y = 0.01
                msg.color.r = 1.0
                msg.color.a = 1.0
                msg.id = i

                for i in range(int(lane.length() * 100.0)):
                    inter = lane.interpolate(i / 100.0)
                    msg.points.append(Point(inter[0], inter[1], 0.0))
                i += 1

                self.lane_pub.publish(msg)

            self.rate.sleep()
Esempio n. 29
0
    def simple_parking(self):
        # here comes the values I will asign to the topics
        # definition of message for speed
        velocidad = NormalizedSpeedCommand()

        # definition of message for steering
        direccion = NormalizedSteeringCommand()

        rospy.sleep(1.0)
        #here are the values assigned to speed amd steering
        # first values
        velocidad.value = -0.2
        direccion.value = 0.7
        self.vel_pub.publish(velocidad)
        self.dir_pub.publish(direccion)
        # use this values for this period of time
        rospy.sleep(2.0)
        # second values
        velocidad.value = -0.2
        direccion.value = -0.7
        self.vel_pub.publish(velocidad)
        self.dir_pub.publish(direccion)
        # use this values for this period of time
        rospy.sleep(1.5)
        #third values
        velocidad.value = 0.2
        direccion.value = 0.2
        self.vel_pub.publish(velocidad)
        self.dir_pub.publish(direccion)
        rospy.sleep(1.0)

        # for stop, in this program the stop was by using ctrl+c
        velocidad.value = 0.0
        direccion.value = 0.0
        self.vel_pub.publish(velocidad)
        self.dir_pub.publish(direccion)
        rospy.sleep(10.0)
Esempio n. 30
0
    def callback(self, data):
        dt = (data.header.stamp - self.last_time).to_sec()
        # 25hz
        if dt < 0.04:
            return

        self.last_time = data.header.stamp
        x = data.pose.pose.position.x
        y = data.pose.pose.position.y
        orientation_q = data.pose.pose.orientation
        orientation_list = [
            orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w
        ]
        (roll, pitch, yaw) = euler_from_quaternion(orientation_list)

        x_index_floor = int(math.floor(x * (100.0 / self.resolution)))
        y_index_floor = int(math.floor(y * (100.0 / self.resolution)))

        x_index_ceil = x_index_floor + 1
        y_index_ceil = y_index_floor + 1

        ceil_ratio_x = x * (100.0 / self.resolution) - x_index_floor
        ceil_ratio_y = y * (100.0 / self.resolution) - y_index_floor

        if x_index_floor < 0:
            x_index_floor = 0
        if x_index_floor > self.map_size_x / self.resolution - 1:
            x_index_floor = self.map_size_x / self.resolution - 1

        if y_index_floor < 0:
            y_index_floor = 0
        if y_index_floor > self.map_size_y / self.resolution - 1:
            y_index_floor = self.map_size_y / self.resolution - 1

        if x_index_ceil < 0:
            x_index_ceil = 0
        if x_index_ceil > self.map_size_x / self.resolution - 1:
            x_index_ceil = self.map_size_x / self.resolution - 1

        if y_index_ceil < 0:
            y_index_ceil = 0
        if y_index_ceil > self.map_size_y / self.resolution - 1:
            y_index_ceil = self.map_size_y / self.resolution - 1

        x3_floor, y3_floor = self.matrix[x_index_floor, y_index_floor, :]
        x3_ceil, y3_ceil = self.matrix[x_index_ceil, y_index_ceil, :]
        x3 = x3_floor * (1.0 - ceil_ratio_x) + x3_ceil * ceil_ratio_x
        y3 = y3_floor * (1.0 - ceil_ratio_y) + y3_ceil * ceil_ratio_y
        f_x = np.cos(yaw) * x3 + np.sin(yaw) * y3
        f_y = -np.sin(yaw) * x3 + np.cos(yaw) * y3

        angle = np.arctan2(f_y, f_x)

        self.integral_error = self.integral_error + angle * dt
        steering = self.Kp * angle + self.Kd * (
            (angle - self.last_angle) / dt) + self.Ki * self.integral_error
        self.last_angle = angle

        if f_x > 0:
            speed = -self.speed_value
        else:
            speed = self.speed_value

        if steering > 1:
            steering = 1

        if steering < -1:
            steering = -1
        if f_x > 0:
            speed = max(self.speed_value,
                        speed * ((np.pi / 3) / (abs(steering) + 1)))

        steerMsg = NormalizedSteeringCommand()
        steerMsg.value = steering
        steerMsg.header.stamp = rospy.Time.now()
        self.pub.publish(steerMsg)

        if not self.shutdown_:
            msg = NormalizedSpeedCommand()
            msg.value = speed
            msg.header.stamp = rospy.Time.now()
            self.pub_speed.publish(msg)