← Back to branch summary

~mrol-dev/mrol/trunk

« back to all changes in this revision

Viewing changes to mrol_mapping/visualiser/rviz.py

  • Committer: Vikas Dhiman
  • Date: 2012-11-16 19:59:46 UTC
  • Revision ID: vikasdhi@buffalo.edu-20121116195946-fwfdqevt3h3eqoyg
Manually added julians changes after merge failed. "Added function to save point cloud in the .ply format"; "Got speed_test working again after API changes.

Show diffs side-by-side

added added

removed removed

Lines of Context:
mrol_mapping/visualiser/rviz.py
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