<gh_stars>1-10
#!/usr/bin/env python
# -*- coding: utf-8 -*-

import cStringIO

import matplotlib

# Execute matplotlib.use('Agg') if using image_server
# http://matplotlib.org/faq/howto_faq.html#matplotlib-in-a-web-application-server
matplotlib.use('Agg')
import matplotlib.pyplot as plt

from threading import Lock
from threading import Thread

import rospy
import time
import arc852.cli_args  as cli
import lidar_cli_args  as lidar_cli
from arc852.constants import LOG_LEVEL
from arc852.cli_args import setup_cli_args
from arc852.constants import HTTP_DELAY_SECS, HTTP_PORT, TEMPLATE_FILE, HTTP_VERBOSE
from lidar_constants import PLOT_ALL, PLOT_CENTROID, PLOT_POINTS, MAX_AXIS_MULT
from lidar_constants import PAUSE, ITERATIONS, MIN_POINTS, THRESHOLD, ITERATIONS_DEFAULT, THRESHOLD_DEFAULT
from lidar_constants import MIN_POINTS_DEFAULT, CONTOUR_TOPIC_DEFAULT, MAX_AXIS_MULT_DEFAULT, PAUSE_DEFAULT
from arc852.image_server_nohost import ImageServer
from arc852.utils import setup_logging
from lidar_navigation.msg import Contour
from point2d import Point2D
from wall_finder import WallFinder


class WallDetector(object):
    def __init__(self,
                 image_server=None,
                 iterations=ITERATIONS_DEFAULT,
                 threshold=THRESHOLD_DEFAULT,
                 min_points=MIN_POINTS_DEFAULT,
                 pause=PAUSE_DEFAULT,
                 plot_all=False,
                 plot_centroid=False,
                 plot_points=False,
                 max_axis_mult=MAX_AXIS_MULT_DEFAULT,
                 contour_topic=CONTOUR_TOPIC_DEFAULT):
        self.__iterations = iterations
        self.__threshold = threshold
        self.__min_points = min_points
        self.__pause = pause
        self.__plot_all = plot_all
        self.__plot_points = plot_points
        self.__plot_centroid = plot_centroid
        self.__max_axis_mult = max_axis_mult
        self.__image_server = image_server

        self.__curr_vals_lock = Lock()
        self.__curr_msg = None
        self.__data_available = False
        self.__stopped = False

        rospy.loginfo("Subscribing to Contour topic {}".format(contour_topic))
        self.__contour_sub = rospy.Subscriber(contour_topic, Contour, self.on_msg)

    def on_msg(self, contour_msg):
        with self.__curr_vals_lock:
            self.__curr_msg = contour_msg
            self.__data_available = True

    def generate_image(self):
        while not self.__stopped:
            if not self.__data_available:
                time.sleep(0.1)
                continue

            with self.__curr_vals_lock:
                contour_msg = self.__curr_msg
                self.__data_available = False

            max_dist = contour_msg.max_dist
            centroid = Point2D(contour_msg.centroid.x, contour_msg.centroid.y)
            all_points = [Point2D(p.x, p.y) for p in contour_msg.all_points]

            if len(all_points) < 2:
                rospy.loginfo("Invalid all_points size: {}".format(len(all_points)))
                continue

            # Initialize plot
            plt.figure(figsize=(8, 8), dpi=80)
            plt.grid(True)

            # Plot robot center
            plt.plot([0], [0], 'r^', markersize=8.0)

            # Plot centroid and write heading
            if self.__plot_centroid or self.__plot_all:
                c = Point2D(centroid.x, centroid.y)
                plt.title("Heading: {} Distance: {}".format(c.heading, round(c.origin_dist, 2)))
                plt.plot([centroid.x], [centroid.y], 'g^', markersize=8.0)

            # Plot point cloud
            if self.__plot_points or self.__plot_all:
                cnt = 0
                walls = []
                for w in WallFinder(iterations=self.__iterations,
                                    threshold=self.__threshold,
                                    min_points=self.__min_points,
                                    points=all_points).walls():
                    m, b = w.slopeYInt()
                    if m > 1 or m < -1:
                        plt.plot([w.xfit(p.y) for p in w.points], [p.y for p in w.points], 'b-')
                        plt.plot([w.p0.x, w.p1.x], [w.p0.y, w.p1.y], 'b^', markersize=6.0)
                        plt.plot([p.x for p in w.points], [p.y for p in w.points], 'bo', markersize=2.0)
                    else:
                        plt.plot([p.x for p in w.points], [w.yfit(p.x) for p in w.points], 'r-')
                        plt.plot([w.p0.x, w.p1.x], [w.p0.y, w.p1.y], 'r^', markersize=6.0)
                        plt.plot([p.x for p in w.points], [p.y for p in w.points], 'ro', markersize=2.0)
                    walls.append(w)

                # if len(walls) != 3:
                print("Found {} walls {} {}".format(len(walls),
                                                    [len(w.points) for w in walls],
                                                    [w.slopeYInt()[0] for w in walls]))

            # Plot axis
            dist = max_dist * self.__max_axis_mult
            plt.axis([-1 * dist, dist, - 0.05, dist])

            if self.__image_server is not None:
                sio = cStringIO.StringIO()
                plt.savefig(sio, format="jpg")
                self.__image_server.image = sio.getvalue()
                sio.close()
            else:
                plt.show()

            # Close resources
            plt.close()

            if self.__pause > 0:
                rospy.sleep(self.__pause)

    def stop(self):
        self.__stopped = True


def main():
    # Parse CLI args
    args = setup_cli_args(lidar_cli.iterations,
                          lidar_cli.min_num_points,
                          lidar_cli.threshold_distance,
                          lidar_cli.pause,
                          lidar_cli.plot_all,
                          lidar_cli.plot_points,
                          lidar_cli.plot_centroid,
                          lidar_cli.max_axis_mult,
                          lidar_cli.contour_topic,
                          ImageServer.args,
                          cli.log_level)

    # Setup logging
    setup_logging(level=args[LOG_LEVEL])

    rospy.init_node('walls_node')

    image_server = ImageServer(template_file=args[TEMPLATE_FILE],
                               http_port=args[HTTP_PORT],
                               http_delay_secs=args[HTTP_DELAY_SECS],
                               http_verbose=args[HTTP_VERBOSE],
                               log_info=rospy.loginfo,
                               log_debug=rospy.logdebug,
                               log_error=rospy.logerror)

    image_server.start()
    image = WallDetector(image_server=image_server,
                         iterations=args[ITERATIONS],
                         min_points=args[MIN_POINTS],
                         threshold=args[THRESHOLD],
                         pause=args[PAUSE],
                         plot_all=args[PLOT_ALL],
                         plot_points=args[PLOT_POINTS],
                         plot_centroid=args[PLOT_CENTROID],
                         max_axis_mult=args[MAX_AXIS_MULT])

    rospy.loginfo("Running")

    try:
        # Running this in a thread will enable Ctrl+C exits
        Thread(target=image.generate_image).start()
        rospy.spin()
    except KeyboardInterrupt:
        pass
    finally:
        image.stop()
        image_server.stop()

    rospy.loginfo("Exiting")


if __name__ == '__main__':
    main()

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import cStringIO

import matplotlib

# Execute matplotlib.use('Agg') if using image_server
# http://matplotlib.org/faq/howto_faq.html#matplotlib-in-a-web-application-server
matplotlib.use('Agg')
import matplotlib.pyplot as plt

from threading import Lock
from threading import Thread

import rospy
import time
import arc852.cli_args  as cli
import lidar_cli_args  as lidar_cli
from arc852.constants import LOG_LEVEL
from arc852.cli_args import setup_cli_args
from arc852.constants import HTTP_DELAY_SECS, HTTP_PORT, TEMPLATE_FILE, HTTP_VERBOSE
from lidar_constants import PLOT_ALL, PLOT_CENTROID, PLOT_POINTS, MAX_AXIS_MULT
from lidar_constants import PAUSE, ITERATIONS, MIN_POINTS, THRESHOLD, ITERATIONS_DEFAULT, THRESHOLD_DEFAULT
from lidar_constants import MIN_POINTS_DEFAULT, CONTOUR_TOPIC_DEFAULT, MAX_AXIS_MULT_DEFAULT, PAUSE_DEFAULT
from arc852.image_server_nohost import ImageServer
from arc852.utils import setup_logging
from lidar_navigation.msg import Contour
from point2d import Point2D
from wall_finder import WallFinder


class WallDetector(object):
    def __init__(self,
                 image_server=None,
                 iterations=ITERATIONS_DEFAULT,
                 threshold=THRESHOLD_DEFAULT,
                 min_points=MIN_POINTS_DEFAULT,
                 pause=PAUSE_DEFAULT,
                 plot_all=False,
                 plot_centroid=False,
                 plot_points=False,
                 max_axis_mult=MAX_AXIS_MULT_DEFAULT,
                 contour_topic=CONTOUR_TOPIC_DEFAULT):
        self.__iterations = iterations
        self.__threshold = threshold
        self.__min_points = min_points
        self.__pause = pause
        self.__plot_all = plot_all
        self.__plot_points = plot_points
        self.__plot_centroid = plot_centroid
        self.__max_axis_mult = max_axis_mult
        self.__image_server = image_server

        self.__curr_vals_lock = Lock()
        self.__curr_msg = None
        self.__data_available = False
        self.__stopped = False

        rospy.loginfo("Subscribing to Contour topic {}".format(contour_topic))
        self.__contour_sub = rospy.Subscriber(contour_topic, Contour, self.on_msg)

    def on_msg(self, contour_msg):
        with self.__curr_vals_lock:
            self.__curr_msg = contour_msg
            self.__data_available = True

    def generate_image(self):
        while not self.__stopped:
            if not self.__data_available:
                time.sleep(0.1)
                continue

            with self.__curr_vals_lock:
                contour_msg = self.__curr_msg
                self.__data_available = False

            max_dist = contour_msg.max_dist
            centroid = Point2D(contour_msg.centroid.x, contour_msg.centroid.y)
            all_points = [Point2D(p.x, p.y) for p in contour_msg.all_points]

            if len(all_points) < 2:
                rospy.loginfo("Invalid all_points size: {}".format(len(all_points)))
                continue

            # Initialize plot
            plt.figure(figsize=(8, 8), dpi=80)
            plt.grid(True)

            # Plot robot center
            plt.plot([0], [0], 'r^', markersize=8.0)

            # Plot centroid and write heading
            if self.__plot_centroid or self.__plot_all:
                c = Point2D(centroid.x, centroid.y)
                plt.title("Heading: {} Distance: {}".format(c.heading, round(c.origin_dist, 2)))
                plt.plot([centroid.x], [centroid.y], 'g^', markersize=8.0)

            # Plot point cloud
            if self.__plot_points or self.__plot_all:
                cnt = 0
                walls = []
                for w in WallFinder(iterations=self.__iterations,
                                    threshold=self.__threshold,
                                    min_points=self.__min_points,
                                    points=all_points).walls():
                    m, b = w.slopeYInt()
                    if m > 1 or m < -1:
                        plt.plot([w.xfit(p.y) for p in w.points], [p.y for p in w.points], 'b-')
                        plt.plot([w.p0.x, w.p1.x], [w.p0.y, w.p1.y], 'b^', markersize=6.0)
                        plt.plot([p.x for p in w.points], [p.y for p in w.points], 'bo', markersize=2.0)
                    else:
                        plt.plot([p.x for p in w.points], [w.yfit(p.x) for p in w.points], 'r-')
                        plt.plot([w.p0.x, w.p1.x], [w.p0.y, w.p1.y], 'r^', markersize=6.0)
                        plt.plot([p.x for p in w.points], [p.y for p in w.points], 'ro', markersize=2.0)
                    walls.append(w)

                # if len(walls) != 3:
                print("Found {} walls {} {}".format(len(walls),
                                                    [len(w.points) for w in walls],
                                                    [w.slopeYInt()[0] for w in walls]))

            # Plot axis
            dist = max_dist * self.__max_axis_mult
            plt.axis([-1 * dist, dist, - 0.05, dist])

            if self.__image_server is not None:
                sio = cStringIO.StringIO()
                plt.savefig(sio, format="jpg")
                self.__image_server.image = sio.getvalue()
                sio.close()
            else:
                plt.show()

            # Close resources
            plt.close()

            if self.__pause > 0:
                rospy.sleep(self.__pause)

    def stop(self):
        self.__stopped = True


def main():
    # Parse CLI args
    args = setup_cli_args(lidar_cli.iterations,
                          lidar_cli.min_num_points,
                          lidar_cli.threshold_distance,
                          lidar_cli.pause,
                          lidar_cli.plot_all,
                          lidar_cli.plot_points,
                          lidar_cli.plot_centroid,
                          lidar_cli.max_axis_mult,
                          lidar_cli.contour_topic,
                          ImageServer.args,
                          cli.log_level)

    # Setup logging
    setup_logging(level=args[LOG_LEVEL])

    rospy.init_node('walls_node')

    image_server = ImageServer(template_file=args[TEMPLATE_FILE],
                               http_port=args[HTTP_PORT],
                               http_delay_secs=args[HTTP_DELAY_SECS],
                               http_verbose=args[HTTP_VERBOSE],
                               log_info=rospy.loginfo,
                               log_debug=rospy.logdebug,
                               log_error=rospy.logerror)

    image_server.start()
    image = WallDetector(image_server=image_server,
                         iterations=args[ITERATIONS],
                         min_points=args[MIN_POINTS],
                         threshold=args[THRESHOLD],
                         pause=args[PAUSE],
                         plot_all=args[PLOT_ALL],
                         plot_points=args[PLOT_POINTS],
                         plot_centroid=args[PLOT_CENTROID],
                         max_axis_mult=args[MAX_AXIS_MULT])

    rospy.loginfo("Running")

    try:
        # Running this in a thread will enable Ctrl+C exits
        Thread(target=image.generate_image).start()
        rospy.spin()
    except KeyboardInterrupt:
        pass
    finally:
        image.stop()
        image_server.stop()

    rospy.loginfo("Exiting")


if __name__ == '__main__':
    main()
#!/usr/bin/env python # -*- coding: utf-8 -*- # Execute matplotlib.use('Agg') if using image_server # http://matplotlib.org/faq/howto_faq.html#matplotlib-in-a-web-application-server # Initialize plot # Plot robot center # Plot centroid and write heading # Plot point cloud # if len(walls) != 3: # Plot axis # Close resources # Parse CLI args # Setup logging # Running this in a thread will enable Ctrl+C exits