Esempio n. 1
0
    def publish_stop_line_msg(self,
                              header,
                              detected=False,
                              at=False,
                              x=0.0,
                              y=0.0):
        """
        Makes and publishes a stop line message.

        Args:
            header: header of a ROS message, usually copied from an incoming message
            detected (`bool`): whether a vehicle has been detected
            at (`bool`): whether we are closer than the desired distance
            x (`float`): distance to the vehicle
            y (`float`): lateral offset from the vehicle (not used, always 0)
        """

        stop_line_msg = StopLineReading()
        stop_line_msg.header = header
        stop_line_msg.stop_line_detected = bool(detected)
        stop_line_msg.at_stop_line = bool(at)
        stop_line_msg.stop_line_point.x = int(x)
        stop_line_msg.stop_line_point.y = int(y)
        self.pub_virtual_stop_line.publish(stop_line_msg)
        """
        Remove once have the road anomaly watcher node
        """
        stopped_flag = BoolStamped()
        stopped_flag.header = header
        stopped_flag.data = bool(at)
        self.pub_stopped_flag.publish(stopped_flag)
    def car_read_action(self):
        while True:
            strhttp = 'http://192.168.0.100/tsp/read_car_action.php?car_id=1'
            req = urllib2.Request(strhttp)
            response = urllib2.urlopen(req)
            the_page = response.read()

            if (self.car_action_check == 1 and int(the_page) == 0
                    and self.last_commodity_tag != 0):
                self.car_action_check = int(the_page)
                e_stop_msg = BoolStamped()
                e_stop_msg.data = int(the_page)
                self.pub_lanefollowing.publish(e_stop_msg)
                if self.last_action == "B":
                    stop_line_reading_msg = StopLineReading()
                    stop_line_reading_msg.stop_line_detected = True
                    stop_line_reading_msg.at_stop_line = 1
                    self.pub_stop_line_reading.publish(stop_line_reading_msg)
                    stop_msg = BoolStamped()
                    stop_msg.data = True
                    self.pub_at_stop_back.publish(stop_msg)
                    time.sleep(1)
                    self.turn = 3
                    self.type_back.publish(self.turn)

            self.last_commodity_tag = 0
            self.car_action_check = int(the_page)
            e_stop_msg = BoolStamped()
            e_stop_msg.data = int(the_page)
            self.pub_lanefollowing.publish(e_stop_msg)
            time.sleep(1)
Esempio n. 3
0
 def after_intersection_work(self):
     self.loginfo("Blocking stop line detection after the intersection")
     stop_line_reading_msg = StopLineReading()
     stop_line_reading_msg.stop_line_detected = False
     stop_line_reading_msg.at_stop_line = False
     self.pub_stop_line_reading.publish(stop_line_reading_msg)
     self.sleep = True
     rospy.sleep(self.off_time.value)
     self.sleep = False
     self.loginfo("Resuming stop line detection after the intersection")
Esempio n. 4
0
 def afterIntersectionWork(self):
     rospy.loginfo("stop line sleep start")
     stop_line_reading_msg = StopLineReading()
     stop_line_reading_msg.stop_line_detected = False
     stop_line_reading_msg.at_stop_line = False
     self.pub_stop_line_reading.publish(stop_line_reading_msg)
     self.sleep = True
     rospy.sleep(self.off_time)
     self.sleep = False
     rospy.loginfo("stop line sleep end")
Esempio n. 5
0
    def test_v_dot_simple(self):
        # publish wheel_cmd_executed
        wheels_cmd = WheelsCmdStamped()
        rate = rospy.Rate(10)
        for i in range(10):
            wheels_cmd.header.stamp = rospy.Time.now()
            wheels_cmd.vel_left = 0.5
            wheels_cmd.vel_right = 0.5
            self.pub_wheels_cmd_executed.publish(wheels_cmd)
            rate.sleep()

        # publish LanePose
        lane_pose = LanePose()
        lane_pose.d = 0
        lane_pose.sigma_d = 0
        lane_pose.phi = 0
        lane_pose.sigma_phi = 0
        lane_pose.status = 0
        lane_pose.in_lane = True
        for i in [1, 2]:
            lane_pose.header.stamp = rospy.Time.now()
            self.pub_lane_pose.publish(lane_pose)
            rate.sleep()

        # publish StopLineReading
        stop_line_reading = StopLineReading()
        stop_line_reading.stop_line_detected = True
        stop_line_reading.at_stop_line = False
        stop_line_reading.stop_line_point = Point()
        for x in [0.51, 0.5]:
            stop_line_reading.header.stamp = rospy.Time.now()
            stop_line_reading.stop_line_point.x = x
            self.pub_stop_line_reading.publish(stop_line_reading)
            rate.sleep()

        # Wait for the odometry_training_pairs_node to finish starting up
        timeout = time.time() + 2.0
        while not self.sub_v_sample.get_num_connections() and \
                not rospy.is_shutdown() and not time.time() > timeout:
            rospy.sleep(0.1)

        msg = self.v_received
        # self.assertEqual(hasattr(msg,'d_L'),True)
        self.assertEqual(msg.d_L, 0.5, 'd_L = %s' % msg.d_L)
        self.assertEqual(msg.d_R, 0.5, 'd_R = %s' % msg.d_R)
        self.assertAlmostEqual(msg.dt, 0.1, 2, 'dt = %s' % msg.dt)
        self.assertEqual(
            msg.theta_angle_pose_delta, 0,
            'theta_angle_pose_delta = %s' % msg.theta_angle_pose_delta)
        self.assertAlmostEqual(msg.x_axis_pose_delta, 0.01, 5,
                               'x = %s' % msg.x_axis_pose_delta)
        self.assertEqual(msg.y_axis_pose_delta, 0,
                         'y = %s' % msg.y_axis_pose_delta)
 def car_read_action(self):
     while True:
         strhttp='http://192.168.0.100/tsp/read_car_action.php?car_id=1'
         req = urllib2.Request(strhttp)
         response = urllib2.urlopen(req)
         the_page = response.read()
         #self.car_action_check=int(the_page)
         """
         if(self.car_action_check!=int(the_page)):
             self.car_action_check=int(the_page)
             e_stop_msg=BoolStamped()
             e_stop_msg.data=int(the_page)
             self.pub_lanefollowing.publish(e_stop_msg)
         """
         #print("yyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyy")
         #print(self.last_commodity_tag)
         if(self.car_action_check==1 and int(the_page)==0 and self.last_commodity_tag!=0):
             self.car_action_check=int(the_page)
             e_stop_msg=BoolStamped()
             e_stop_msg.data=int(the_page)
             self.pub_lanefollowing.publish(e_stop_msg)
             print("lllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllll")
             #print(self.last_commodity_tag)
             #strhttp='http://192.168.0.100/tsp/car_recode_navigation.php?car_id=1&tag_id='+str(self.last_commodity_tag)
             #req = urllib2.Request(strhttp)
             #response = urllib2.urlopen(req)
             #the_page_1 = response.read()
             #print(the_page_1)
             print(self.last_action)
             #print("Sssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssss")
             if self.last_action=="B":
                 stop_line_reading_msg = StopLineReading()
                 stop_line_reading_msg.stop_line_detected = True
                 stop_line_reading_msg.at_stop_line = 1
                 self.pub_stop_line_reading.publish(stop_line_reading_msg)
                 print("ooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo")
                 stop_msg=BoolStamped()
                 stop_msg.data=True
                 self.pub_at_stop_back.publish(stop_msg)
                 time.sleep(5)
                 #print("ttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttt")
                 self.turn=3
                 self.type_back.publish(self.turn)
             self.last_commodity_tag=0
         if(self.car_action_check!=int(the_page)):
         #if(True):
             self.car_action_check=int(the_page)
             e_stop_msg=BoolStamped()
             e_stop_msg.data=int(the_page)
             self.pub_lanefollowing.publish(e_stop_msg)
    def test_v_dot_simple(self):
        # publish wheel_cmd_executed
        wheels_cmd = WheelsCmdStamped()
        rate = rospy.Rate(10)
        for i in range(10):
            wheels_cmd.header.stamp = rospy.Time.now()
            wheels_cmd.vel_left = 0.5
            wheels_cmd.vel_right = 0.5
            self.pub_wheels_cmd_executed.publish(wheels_cmd)
            rate.sleep()

        # publish LanePose
        lane_pose = LanePose()
        lane_pose.d = 0
        lane_pose.sigma_d = 0
        lane_pose.phi = 0
        lane_pose.sigma_phi = 0
        lane_pose.status = 0
        lane_pose.in_lane = True
        for i in [1, 2]:
            lane_pose.header.stamp = rospy.Time.now()
            self.pub_lane_pose.publish(lane_pose)
            rate.sleep()

        # publish StopLineReading
        stop_line_reading = StopLineReading()
        stop_line_reading.stop_line_detected = True
        stop_line_reading.at_stop_line = False
        stop_line_reading.stop_line_point = Point()
        for x in [0.51, 0.5]:
            stop_line_reading.header.stamp = rospy.Time.now()
            stop_line_reading.stop_line_point.x = x
            self.pub_stop_line_reading.publish(stop_line_reading)
            rate.sleep()

        # Wait for the odometry_training_pairs_node to finish starting up
        timeout = time.time()+2.0
        while not self.sub_v_sample.get_num_connections() and \
                not rospy.is_shutdown() and not time.time() > timeout:
            rospy.sleep(0.1)

        msg = self.v_received
        # self.assertEqual(hasattr(msg,'d_L'),True)
        self.assertEqual(msg.d_L, 0.5, 'd_L = %s' % msg.d_L)
        self.assertEqual(msg.d_R, 0.5, 'd_R = %s' % msg.d_R)
        self.assertAlmostEqual(msg.dt, 0.1, 2,'dt = %s' % msg.dt)
        self.assertEqual(msg.theta_angle_pose_delta, 0, 'theta_angle_pose_delta = %s' % msg.theta_angle_pose_delta)
        self.assertAlmostEqual(msg.x_axis_pose_delta, 0.01, 5, 'x = %s' % msg.x_axis_pose_delta)
        self.assertEqual(msg.y_axis_pose_delta, 0, 'y = %s' % msg.y_axis_pose_delta)
    def __init__(self):
        # Save the name of the node
        self.node_name = rospy.get_name()
        self.mode = None
        self.turn_type = -1
        self.in_lane = False
        self.lane_pose = LanePose()
        self.stop_line_reading = StopLineReading()

        self.trajectory_reparam = rospy.get_param("~trajectory_reparam",1)
        self.pub_cmd = rospy.Publisher("~car_cmd",Twist2DStamped,queue_size=1)
        self.pub_done = rospy.Publisher("~intersection_done",BoolStamped,queue_size=1)

        # Construct maneuvers
        self.maneuvers = dict()

        self.maneuvers[0] = self.getManeuver("turn_left")
        self.maneuvers[1] = self.getManeuver("turn_forward")
        self.maneuvers[2] = self.getManeuver("turn_right")
        self.maneuvers[3] = self.getManeuver("turn_back")
        # self.maneuvers[-1] = self.getManeuver("turn_stop")

        self.srv_turn_left = rospy.Service("~turn_left", Empty, self.cbSrvLeft)
        self.srv_turn_right = rospy.Service("~turn_right", Empty, self.cbSrvRight)
        self.srv_turn_forward = rospy.Service("~turn_forward", Empty, self.cbSrvForward)
        self.srv_turn_back = rospy.Service("~turn_back", Empty, self.cbSrvBack)
        self.rate = rospy.Rate(30)

        # Subscribers
        self.sub_in_lane = rospy.Subscriber("~in_lane", BoolStamped, self.cbInLane, queue_size=1)
        self.sub_turn_type = rospy.Subscriber("~turn_type", Int16, self.cbTurnType, queue_size=1)
        self.sub_mode = rospy.Subscriber("~mode", FSMState, self.cbFSMState, queue_size=1)
        self.sub_lane_pose = rospy.Subscriber("~lane_pose", LanePose, self.cbLanePose, queue_size=1)
        self.sub_stop_line = rospy.Subscriber("~stop_line_reading", StopLineReading, self.cbStopLine, queue_size=1)
Esempio n. 9
0
    def cb_segments(self, segment_list_msg):

        if not self.switch or self.sleep:
            return

        good_seg_count = 0
        stop_line_x_accumulator = 0.0
        stop_line_y_accumulator = 0.0
        for segment in segment_list_msg.segments:
            if segment.color != segment.RED:
                continue
            if segment.points[0].x < 0 or segment.points[1].x < 0:  # the point is behind us
                continue

            p1_lane = self.to_lane_frame(segment.points[0])
            p2_lane = self.to_lane_frame(segment.points[1])
            avg_x = 0.5 * (p1_lane[0] + p2_lane[0])
            avg_y = 0.5 * (p1_lane[1] + p2_lane[1])
            stop_line_x_accumulator += avg_x
            stop_line_y_accumulator += avg_y  # TODO output covariance and not just mean
            good_seg_count += 1.0

        stop_line_reading_msg = StopLineReading()
        stop_line_reading_msg.header.stamp = segment_list_msg.header.stamp
        if good_seg_count < self.min_segs.value:
            stop_line_reading_msg.stop_line_detected = False
            stop_line_reading_msg.at_stop_line = False
            self.pub_stop_line_reading.publish(stop_line_reading_msg)

            # ### CRITICAL: publish false to at stop line output_topic
            # msg = BoolStamped()
            # msg.header.stamp = stop_line_reading_msg.header.stamp
            # msg.data = False
            # self.pub_at_stop_line.publish(msg)
            # ### CRITICAL END

        else:
            stop_line_reading_msg.stop_line_detected = True
            stop_line_point = Point()
            stop_line_point.x = stop_line_x_accumulator / good_seg_count
            stop_line_point.y = stop_line_y_accumulator / good_seg_count
            stop_line_reading_msg.stop_line_point = stop_line_point
            # Only detect redline if y is within max_y distance:
            stop_line_reading_msg.at_stop_line = (
                stop_line_point.x < self.stop_distance.value and np.abs(stop_line_point.y) < self.max_y.value
            )

            self.pub_stop_line_reading.publish(stop_line_reading_msg)
            if stop_line_reading_msg.at_stop_line:
                msg = BoolStamped()
                msg.header.stamp = stop_line_reading_msg.header.stamp
                msg.data = True
                self.pub_at_stop_line.publish(msg)
    def processSegments(
        self, segment_list_msg
    ):  #Process segment list and decide whether there is a stop line.
        if not self.active or self.sleep:
            return
        good_seg_count = 0
        stop_line_x_accumulator = 0.0
        stop_line_y_accumulator = 0.0
        for segment in segment_list_msg.segments:
            if segment.color != segment.RED:
                continue
            if segment.points[0].x < 0 or segment.points[
                    1].x < 0:  # the point is behind us
                continue
            ###Transform segment list to lane pose frame
            p1_lane = self.to_lane_frame(segment.points[0])
            p2_lane = self.to_lane_frame(segment.points[1])
            ###Find average x and y of the red line segment
            avg_x = 0.5 * (p1_lane[0] + p2_lane[0])
            avg_y = 0.5 * (p1_lane[1] + p2_lane[1])
            stop_line_x_accumulator += avg_x
            stop_line_y_accumulator += avg_y  # TODO output covariance and not just mean
            good_seg_count += 1.0

        stop_line_reading_msg = StopLineReading()
        stop_line_reading_msg.header.stamp = segment_list_msg.header.stamp
        if (
                good_seg_count < self.min_segs
        ):  #If red line segment is lower that threshold, then, there is no stop line
            stop_line_reading_msg.stop_line_detected = False
            stop_line_reading_msg.at_stop_line = False
            self.pub_stop_line_reading.publish(stop_line_reading_msg)
            return
        else:
            stop_line_reading_msg.stop_line_detected = True
            stop_line_point = Point()
            stop_line_point.x = stop_line_x_accumulator / good_seg_count
            stop_line_point.y = stop_line_y_accumulator / good_seg_count
            stop_line_reading_msg.stop_line_point = stop_line_point
            stop_line_reading_msg.at_stop_line = stop_line_point.x < self.stop_distance and math.fabs(
                stop_line_point.y) < self.lanewidth / 4
            self.pub_stop_line_reading.publish(stop_line_reading_msg)
            if stop_line_reading_msg.at_stop_line:
                ###Task: assign fsm stop line message, if message data is true, then it will trigger stop event
                ###Problem
                msg = BoolStamped()  #Declaration
                msg.header.stamp = segment_list_msg.header.stamp  #Assign segment_list time stamp
                msg.data = true  #If you want to trigger the event at_stop_line, message data must be "true"

                print "Event stop line fiter is triggered ? :" + msg.data

                self.pub_at_stop_line.publish(msg)
Esempio n. 11
0
    def processSegments(self, segment_list_msg):
        #print ("-*/*/*/*/*&&&&&&&&&&*/*/*/*/-")
        if not self.active or self.sleep:
            return
        good_seg_count = 0
        stop_line_x_accumulator = 0.0
        stop_line_y_accumulator = 0.0
        for segment in segment_list_msg.segments:
            if segment.color != segment.RED and segment.color != segment.BLUE:
                continue
            if segment.points[0].x < 0 or segment.points[
                    1].x < 0:  # the point is behind us
                continue

            p1_lane = self.to_lane_frame(segment.points[0])
            p2_lane = self.to_lane_frame(segment.points[1])
            avg_x = 0.5 * (p1_lane[0] + p2_lane[0])
            avg_y = 0.5 * (p1_lane[1] + p2_lane[1])
            stop_line_x_accumulator += avg_x
            stop_line_y_accumulator += avg_y  # TODO output covariance and not just mean
            good_seg_count += 1.0

        stop_line_reading_msg = StopLineReading()
        stop_line_reading_msg.header.stamp = segment_list_msg.header.stamp
        if (good_seg_count < self.min_segs):
            stop_line_reading_msg.stop_line_detected = False
            stop_line_reading_msg.at_stop_line = False
            self.pub_stop_line_reading.publish(stop_line_reading_msg)
            return

        stop_line_reading_msg.stop_line_detected = True
        stop_line_point = Point()
        stop_line_point.x = stop_line_x_accumulator / good_seg_count
        stop_line_point.y = stop_line_y_accumulator / good_seg_count
        stop_line_reading_msg.stop_line_point = stop_line_point
        stop_line_reading_msg.at_stop_line = stop_line_point.x < self.stop_distance and math.fabs(
            stop_line_point.y) < self.lanewidth / 4
        self.pub_stop_line_reading.publish(stop_line_reading_msg)
        if stop_line_reading_msg.at_stop_line:
            msg = BoolStamped()
            msg.header.stamp = stop_line_reading_msg.header.stamp
            msg.data = True
            print "-------at stop line-------"
            if segment.color == segment.BLUE:
                self.pub_sendmessage = rospy.Publisher(
                    "/arg2/stop_line_filter_node/send",
                    BoolStamped,
                    queue_size=1)
                self.pub_sendmessage.publish(msg)
            elif segment.color == segment.RED:
                self.pub_at_stop_line.publish(msg)
    def processSegments(self, segment_list_msg):
        good_seg_count = 0
        stop_line_x_accumulator = 0.0
        stop_line_y_accumulator = 0.0
        for segment in segment_list_msg.segments:
            if segment.color != segment.RED:
                continue
            if segment.points[0].x < 0 or segment.points[
                    1].x < 0:  # the point is behind us
                continue

            if not self.active or self.sleep:
                continue

            p1_lane = self.to_lane_frame(segment.points[0])
            p2_lane = self.to_lane_frame(segment.points[1])
            avg_x = 0.5 * (p1_lane[0] + p2_lane[0])
            avg_y = 0.5 * (p1_lane[1] + p2_lane[1])
            stop_line_x_accumulator += avg_x
            stop_line_y_accumulator += avg_y  # TODO output covariance and not just mean
            good_seg_count += 1.0

        stop_line_reading_msg = StopLineReading()
        stop_line_reading_msg.header.stamp = segment_list_msg.header.stamp
        if (good_seg_count < self.min_segs):
            stop_line_reading_msg.stop_line_detected = False
            stop_line_reading_msg.at_stop_line = False
            self.pub_stop_line_reading.publish(stop_line_reading_msg)
            return

        stop_line_reading_msg.stop_line_detected = True
        stop_line_point = Point()
        stop_line_point.x = stop_line_x_accumulator / good_seg_count
        stop_line_point.y = stop_line_y_accumulator / good_seg_count
        stop_line_reading_msg.stop_line_point = stop_line_point
        stop_line_reading_msg.at_stop_line = stop_line_point.x < self.stop_distance and math.fabs(
            stop_line_point.y) < self.lanewidth / 4
        self.pub_stop_line_reading.publish(stop_line_reading_msg)
        if stop_line_reading_msg.at_stop_line:
            msg = BoolStamped()
            msg.header.stamp = stop_line_reading_msg.header.stamp
            msg.data = True
            self.pub_at_stop_line.publish(msg)
        else:
            msg = BoolStamped()
            msg.header.stamp = stop_line_reading_msg.header.stamp
            msg.data = False
            self.pub_at_stop_line.publish(msg)
    def processSegments(self, segment_list_msg):
        good_seg_count=0
        stop_line_x_accumulator=0.0
        stop_line_y_accumulator=0.0
        for segment in segment_list_msg.segments:
            if segment.color != segment.RED:
                continue
            if segment.points[0].x < 0 or segment.points[1].x < 0: # the point is behind us 
                continue
	
 	    if not self.active or self.sleep:
            	continue

            p1_lane = self.to_lane_frame(segment.points[0])
            p2_lane = self.to_lane_frame(segment.points[1])
            avg_x = 0.5*(p1_lane[0] + p2_lane[0])
            avg_y = 0.5*(p1_lane[1] + p2_lane[1])
            stop_line_x_accumulator += avg_x
            stop_line_y_accumulator += avg_y # TODO output covariance and not just mean
            good_seg_count += 1.0

        stop_line_reading_msg = StopLineReading()
        stop_line_reading_msg.header.stamp = segment_list_msg.header.stamp
        if (good_seg_count < self.min_segs):
            stop_line_reading_msg.stop_line_detected = False
            stop_line_reading_msg.at_stop_line = False
            self.pub_stop_line_reading.publish(stop_line_reading_msg)
            return
        
        stop_line_reading_msg.stop_line_detected = True
        stop_line_point = Point()
        stop_line_point.x = stop_line_x_accumulator/good_seg_count
        stop_line_point.y = stop_line_y_accumulator/good_seg_count
        stop_line_reading_msg.stop_line_point = stop_line_point
        stop_line_reading_msg.at_stop_line = stop_line_point.x < self.stop_distance and math.fabs(stop_line_point.y) < self.lanewidth/4 
        self.pub_stop_line_reading.publish(stop_line_reading_msg)    
        if stop_line_reading_msg.at_stop_line:
            msg = BoolStamped()
            msg.header.stamp = stop_line_reading_msg.header.stamp
            msg.data = True
            self.pub_at_stop_line.publish(msg)
        else:
            msg = BoolStamped()
            msg.header.stamp = stop_line_reading_msg.header.stamp
            msg.data = False
            self.pub_at_stop_line.publish(msg)
Esempio n. 14
0
    def __init__(self):
        self.node_name = rospy.get_name()

        ## Parameters
        self.stop_line_max_age = self.setupParameter(
            "~stop_line_max_age", 1.0
        )  # [sec] don't use stop_line_reading msg pairs more than this far apart
        self.lane_pose_max_age = self.setupParameter(
            "~lane_pose_max_age", 1.0
        )  # [sec] don't use lane_pose msg pairs more than this far apart
        self.wheels_cmd_max_age = self.setupParameter(
            "~wheels_cmd_max_age",
            2.0)  # [sec] throw away wheels_cmd's older than this

        ## state vars
        self.in_lane = False
        self.current_wheels_cmd = WheelsCmdStamped()
        self.old_stop_line_msg = StopLineReading()
        self.old_lane_pose_msg = LanePose()
        self.cmd_buffer = deque()  # buffer of wheel commands

        self.v_sample_msg = Vsample(
        )  # global variable because different parts of this are set in different callbacks

        ## publishers and subscribers
        self.sub_stop_line_reading = rospy.Subscriber("~stop_line_reading",
                                                      StopLineReading,
                                                      self.stopLineCB)
        self.sub_lane_pose = rospy.Subscriber("~lane_pose", LanePose,
                                              self.lanePoseCB)
        self.sub_wheels_cmd_executed = rospy.Subscriber(
            "~wheels_cmd_executed", WheelsCmdStamped, self.wheelsCmdCB)
        self.pub_v_sample = rospy.Publisher("~v_sample", Vsample, queue_size=1)
        self.pub_theta_dot_sample = rospy.Publisher("~theta_dot_sample",
                                                    ThetaDotSample,
                                                    queue_size=1)
        rospy.loginfo('[%s] Initialized' % self.node_name)
Esempio n. 15
0
 if(self.car_action_check==1 and int(the_page)==0 and self.last_commodity_tag!=0):
     self.car_action_check=int(the_page)
     e_stop_msg=BoolStamped()
     e_stop_msg.data=int(the_page)
     self.pub_lanefollowing.publish(e_stop_msg)
     print("lllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllll")
     #print(self.last_commodity_tag)
     #strhttp='http://192.168.0.100/tsp/car_recode_navigation.php?car_id=1&tag_id='+str(self.last_commodity_tag)
     #req = urllib2.Request(strhttp)
     #response = urllib2.urlopen(req)
     #the_page_1 = response.read()
     #print(the_page_1)
     print(self.last_action)
     #print("Sssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssss")
     if self.last_action=="B":
         stop_line_reading_msg = StopLineReading()
         stop_line_reading_msg.stop_line_detected = True
         stop_line_reading_msg.at_stop_line = 1
         self.pub_stop_line_reading.publish(stop_line_reading_msg)
         print("ooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo")
         stop_msg=BoolStamped()
         stop_msg.data=True
         self.pub_at_stop_back.publish(stop_msg)
         time.sleep(5)
         #print("ttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttttt")
         self.turn=3
         self.type_back.publish(self.turn)
     self.last_commodity_tag=0
 if(self.car_action_check!=int(the_page)):
 #if(True):
     self.car_action_check=int(the_page)