~wecacuee/mrol/mrol-dev

« back to all changes in this revision

Viewing changes to mrol_mapping/visualiser/rviz.py

  • Committer: Vikas Dhiman
  • Date: 2012-11-20 22:12:16 UTC
  • mfrom: (0.5.3 mrol)
  • Revision ID: vikasdhi@buffalo.edu-20121120221216-nunda3pv0n8jsf4e
MergingĀ lp:~wecacuee/mrol/mrol-devĀ 

Show diffs side-by-side

added added

removed removed

Lines of Context:
 
1
import roslib;roslib.load_manifest('mrol_ros')
 
2
import rospy
 
3
import tf
 
4
import tf.transformations
 
5
import rosutils_mrol.rosutils_mrol as rosutils
 
6
import sensor_msgs
 
7
 
 
8
import signal
 
9
import time
 
10
import sys
 
11
import os
 
12
import numpy as np
 
13
import mrol_mapping.log as log
 
14
import logging
 
15
 
 
16
from thread import allocate_lock
 
17
from threading import Thread
 
18
 
 
19
TRAJECTORY = "/trajectory"
 
20
GLOBAL_FRAME = "/map"
 
21
MROL_TOPIC = "/mrol"
 
22
 
 
23
logger = log.getLogger('visualiser.rviz')
 
24
logger.setLevel(logging.DEBUG)
 
25
 
 
26
class Visualiser:
 
27
    seq = 0
 
28
    def __init__(self, npts=100000, Rotate=False, title=''):
 
29
        self.point_cloud = None
 
30
        self.point_cloud_lock = allocate_lock()
 
31
 
 
32
        self.trajectory_point = tf.transformations.identity_matrix()
 
33
        self.trajectory_point_lock = allocate_lock()
 
34
 
 
35
        inthandler = signal.getsignal(signal.SIGINT)
 
36
        # overrides signal handler
 
37
        rospy.init_node('visualiser', anonymous=True)
 
38
        def newhandler(signum, frame):
 
39
            os._exit(0)
 
40
        signal.signal(signal.SIGINT, newhandler)
 
41
        t = Thread(target=self._publisher_thread)
 
42
        t.daemon = True # die when no other non-daemon thread is around
 
43
        t.start()
 
44
        t = Thread(target=self._transform_thread)
 
45
        t.daemon = True # die when no other non-daemon thread is around
 
46
        t.start()
 
47
 
 
48
    def _transform_thread(self):
 
49
        br = tf.TransformBroadcaster()
 
50
        logger.debug("****** Started transform thread *****")
 
51
        try:
 
52
            while not rospy.is_shutdown():
 
53
                with self.trajectory_point_lock:
 
54
                    if self.trajectory_point is None:
 
55
                        continue
 
56
 
 
57
                with self.trajectory_point_lock:
 
58
                    translation = tuple(self.trajectory_point[:3, 3])
 
59
                    quat = tf.transformations.quaternion_from_matrix(
 
60
                        self.trajectory_point)
 
61
                    br.sendTransform(translation,
 
62
                                     quat,
 
63
                                     rospy.Time.now(),
 
64
                                     TRAJECTORY,
 
65
                                     GLOBAL_FRAME)
 
66
                time.sleep(0.1)
 
67
        except Exception:
 
68
            os._exit(1)
 
69
 
 
70
    def _publisher_thread(self):
 
71
        pub = rospy.Publisher(MROL_TOPIC, sensor_msgs.msg.PointCloud2)
 
72
        logger.debug("Started publishing thread")
 
73
        rate = rospy.Rate(1.0)
 
74
        try:
 
75
            while not rospy.is_shutdown():
 
76
                with self.point_cloud_lock:
 
77
                    if self.point_cloud is not None:
 
78
                        pub.publish(self.point_cloud)
 
79
                time.sleep(1)
 
80
        except Exception:
 
81
            os_.exit(1)
 
82
 
 
83
    def addmappts(self, xyzs, colour=None):
 
84
        """Display xyzs"""
 
85
        with self.point_cloud_lock:
 
86
            self.point_cloud = rosutils.xyzs2pc2(
 
87
                xyzs, rospy.Time.now(), self.seq, frame_id=GLOBAL_FRAME)
 
88
            self.seq = self.seq + 1
 
89
 
 
90
    def setrobotpose(self, transform_mat):
 
91
        """Display robot position with given pose"""
 
92
        with self.trajectory_point_lock:
 
93
            self.trajectory_point[:3, :3] = transform_mat[:3, :3]
 
94
 
 
95
    def addtrajectorypoint(self, xyz):
 
96
        """Display robot position at given coordinate"""
 
97
 
 
98
        with self.trajectory_point_lock:
 
99
            self.trajectory_point[:3, 3] = np.array(xyz)
 
100
 
 
101
    def getkeypress(self):
 
102
        pass
 
103
    
 
104
    def setminmax(self, colourmin, colourmax):
 
105
        pass
 
106
    
 
107
    def setautominmax(self, Bool=True):
 
108
        pass
 
109
 
 
110
    def set_orthographic(self, Bool=False):
 
111
        pass
 
112
 
 
113
    def setleftpts(self, xyzs, M=None):
 
114
        pass
 
115
 
 
116
    def setrightpts(self, xyzs, M=None):
 
117
        pass
 
118
 
 
119
    def setspinpts(self, xyzs, M=None):
 
120
        pass
 
121
 
 
122
    def clear(self):
 
123
        pass
 
124
 
 
125
    def setinfostr(self, labeltext):
 
126
        pass
 
127