REC RPC library
ROSHelper.h
Go to the documentation of this file.
1 /*
2 Copyright (c) 2011, REC Robotics Equipment Corporation GmbH, Planegg, Germany
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without modification,
6 are permitted provided that the following conditions are met:
7 
8 - Redistributions of source code must retain the above copyright notice,
9  this list of conditions and the following disclaimer.
10 - Redistributions in binary form must reproduce the above copyright notice,
11  this list of conditions and the following disclaimer in the documentation and/or
12  other materials provided with the distribution.
13 - Neither the name of the REC Robotics Equipment Corporation GmbH nor the names of
14  its contributors may be used to endorse or promote products derived from this software
15  without specific prior written permission.
16 
17 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
18 OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
19 AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
20 CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
21 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
22 DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
23 IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
24 OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
25 */
26 
27 #ifndef _REC_RPC_ROSHELPER_H_
28 #define _REC_RPC_ROSHELPER_H_
29 
30 #include "rec/rpc/defines.h"
31 #include "rec/rpc/utils.h"
32 
33 #include <QtCore>
34 #include <QMetaType>
35 #include <QPixmap>
36 #include <QImage>
37 #include <QRgb>
38 #include <QVector>
39 
40 #include <ros/ros.h>
41 #include <tf/transform_listener.h>
42 #include <geometry_msgs/Point.h>
43 #include <sensor_msgs/PointCloud.h>
44 
45 #include <nav_msgs/OccupancyGrid.h>
46 
47 namespace rec
48 {
49  namespace rpc
50  {
51  class MapInfo
52  {
53  public:
55  : resolution( 0.0f )
56  , frame_id( "/map" )
57  {
58  offset[0] = 0.0f;
59  offset[1] = 0.0f;
60  }
61 
62  MapInfo( float resolution_, float offsetX, float offsetY, std::string frame_id_ = "/map" )
63  : resolution( resolution_ )
64  , frame_id( "/map" )
65  {
66  offset[0] = offsetX;
67  offset[1] = offsetY;
68  }
69 
70  MapInfo( const nav_msgs::OccupancyGridConstPtr& msg, std::string frame_id_ = "/map" )
71  {
72  resolution = msg->info.resolution;
73  offset[0] = -((int)msg->info.width) - (msg->info.origin.position.x / resolution); // remember: horizontal mirroring!
74  offset[1] = msg->info.origin.position.y / resolution;
75  frame_id = frame_id_;
76  }
77 
78  bool isEmpty() const { return 0.0f == resolution; }
79 
80  void clear() { resolution = 0.0f; }
81 
82  float resolution;
83  float offset[2];
84  std::string frame_id;
85  };
86 
87  static REC_RPC_FUNCTION_IS_NOT_USED QPointF getPoint( const MapInfo& mapInfo, const tf::Pose& p )
88  {
89  QPointF point;
90  point.setX( -1.0 / mapInfo.resolution * p.getOrigin()[0] - mapInfo.offset[0] );
91  point.setY( 1.0 / mapInfo.resolution * p.getOrigin()[1] - mapInfo.offset[1] );
92  return point;
93  }
94 
95  static REC_RPC_FUNCTION_IS_NOT_USED QPointF getPoint( const MapInfo& mapInfo, const geometry_msgs::Point& p )
96  {
97  QPointF point;
98  point.setX( -1.0 / mapInfo.resolution * p.x - mapInfo.offset[0] );
99  point.setY( 1.0 / mapInfo.resolution * p.y - mapInfo.offset[1] );
100  return point;
101  }
102 
103  static REC_RPC_FUNCTION_IS_NOT_USED QVector< QPointF > getPoints( tf::TransformListener* tf, const MapInfo& mapInfo, const sensor_msgs::PointCloud& pointCloudIn )
104  {
105  QVector< QPointF > points;
106 
107  try
108  {
109  tf::StampedTransform transform;
110  tf->lookupTransform( mapInfo.frame_id, pointCloudIn.header.frame_id, ros::Time(0), transform );
111 
112  points.resize( pointCloudIn.points.size() );
113 
114  for( unsigned int i=0; i<pointCloudIn.points.size(); ++i )
115  {
116  tf::Pose p;
117  p.setOrigin( btVector3( pointCloudIn.points[i].x, pointCloudIn.points[i].y, 0 ) );
118 
119  p = transform * p;
120 
121  points[i] = getPoint( mapInfo, p );
122  }
123  }
124  catch(tf::LookupException& ex) {
125  ROS_ERROR("No Transform available Error: %s\n", ex.what());
126  }
127  catch(tf::ConnectivityException& ex) {
128  ROS_ERROR("Connectivity Error: %s\n", ex.what());
129  }
130  catch(tf::ExtrapolationException& ex) {
131  ROS_ERROR("Extrapolation Error: %s\n", ex.what());
132  }
133 
134  return points;
135  }
136 
137  static REC_RPC_FUNCTION_IS_NOT_USED double getYaw( const tf::Pose& p )
138  {
139  btScalar useless_pitch, useless_roll, yaw;
140  p.getBasis().getRPY( useless_roll, useless_pitch, yaw );
141 
142  double x = cos( yaw );
143  double y = sin( yaw );
144 
145  return rad2deg( atan2( y, -x ) );
146  }
147 
148  static REC_RPC_FUNCTION_IS_NOT_USED bool poseStampedToMap( tf::TransformListener* tf, const MapInfo& mapInfo, const geometry_msgs::PoseStamped& pose, QPointF* point, double* rotation_deg )
149  {
150  try
151  {
152  tf::StampedTransform transform;
153  tf->lookupTransform( mapInfo.frame_id, pose.header.frame_id, ros::Time(0), transform );
154 
155  tf::Pose p;
156  p.setOrigin( btVector3( pose.pose.position.x, pose.pose.position.y, pose.pose.position.z ) );
157  p.setRotation( btQuaternion( pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w ) );
158 
159  p = transform * p;
160 
161  if( rotation_deg )
162  {
163  *rotation_deg = getYaw( p );
164  }
165 
166  *point = getPoint( mapInfo, p );
167  return true;
168  }
169  catch(tf::LookupException& ex) {
170  ROS_ERROR("No Transform available Error: %s\n", ex.what());
171  return false;
172  }
173  catch(tf::ConnectivityException& ex) {
174  ROS_ERROR("Connectivity Error: %s\n", ex.what());
175  return false;
176  }
177  catch(tf::ExtrapolationException& ex) {
178  ROS_ERROR("Extrapolation Error: %s\n", ex.what());
179  return false;
180  }
181 
182  return false;
183  }
184 
185  static REC_RPC_FUNCTION_IS_NOT_USED bool poseToMap( tf::TransformListener* tf, const MapInfo& mapInfo, const geometry_msgs::Pose& pose, const std::string& sourceFrame, const ros::Time& time, QPointF* point, double* rotation_deg )
186  {
187  geometry_msgs::PoseStamped poseStamped;
188  poseStamped.header.frame_id = sourceFrame;
189  poseStamped.header.stamp = time;
190  poseStamped.pose = pose;
191 
192  if( poseStampedToMap( tf, mapInfo, poseStamped, point, rotation_deg ) )
193  {
194  return true;
195  }
196  else
197  {
198  return false;
199  }
200  }
201 
202  static REC_RPC_FUNCTION_IS_NOT_USED bool toMap( tf::TransformListener* tf, const MapInfo& mapInfo, const geometry_msgs::Point& position, const std::string& sourceFrame, const ros::Time& time, QPointF* point )
203  {
204  geometry_msgs::PoseStamped pose;
205  pose.header.frame_id = sourceFrame;
206  pose.header.stamp = time;
207  pose.pose.position = position;
208  pose.pose.orientation.w = 1.0;
209 
210  if( poseStampedToMap( tf, mapInfo, pose, point, NULL ) )
211  {
212  return true;
213  }
214  else
215  {
216  return false;
217  }
218  }
219 
220  static REC_RPC_FUNCTION_IS_NOT_USED bool toMap( tf::TransformListener* tf, const MapInfo& mapInfo, const geometry_msgs::Pose& pose, const std::string& sourceFrame, const ros::Time& time, QPointF* point, double* rotation_deg )
221  {
222  geometry_msgs::PoseStamped poseStamped;
223  poseStamped.header.frame_id = sourceFrame;
224  poseStamped.header.stamp = time;
225  poseStamped.pose = pose;
226 
227  if( poseStampedToMap( tf, mapInfo, poseStamped, point, rotation_deg ) )
228  {
229  return true;
230  }
231  else
232  {
233  return false;
234  }
235  }
236 
237  static REC_RPC_FUNCTION_IS_NOT_USED geometry_msgs::Pose toPose( const MapInfo& mapInfo, const QPointF& point, double rot_degrees )
238  {
239  geometry_msgs::Pose pose;
240  pose.position.x = ( - mapInfo.resolution * ( point.x() + mapInfo.offset[0] ) );
241  pose.position.y = ( mapInfo.resolution * ( point.y() + mapInfo.offset[1] ) );
242  pose.position.z = 0;
243 
244  double rot = deg2rad( rot_degrees );
245  double x = cos( rot );
246  double y = sin( rot );
247  rot = atan2( y, -x );
248 
249  tf::Quaternion q = tf::createQuaternionFromYaw( rot );
250 
251  pose.orientation.x = q.x();
252  pose.orientation.y = q.y();
253  pose.orientation.z = q.z();
254  pose.orientation.w = q.w();
255 
256  return pose;
257  }
258 
259  static REC_RPC_FUNCTION_IS_NOT_USED QImage fromOccupancyGrid( const nav_msgs::OccupancyGridConstPtr& msg )
260  {
261  QImage imgBuffer2( (unsigned char*)&(*msg->data.begin()), msg->info.width, msg->info.height, msg->info.width * sizeof( int8_t ), QImage::Format_Indexed8 );
262  QImage imgBuffer( imgBuffer2.copy() );
263  QVector<QRgb> colorTable;
264  unsigned int i = 0;
265  for( i = 0; i < 101; ++i )
266  {
267  QRgb col = 0xa0000000;
268  int val = (100 - i) / 100 * 255;
269  col |= val | (val << 8) | (val << 16);
270  colorTable.push_back(col);
271  }
272  for( /*i*/; i < 255; ++i )
273  {
274  colorTable.push_back( 0xFFFF0000 ); // red for warning
275  }
276  colorTable.push_back( 0xFF888888 ); // grey for 255 = -1 = "unknown"
277  imgBuffer.setColorTable( colorTable );
278 
279  return imgBuffer;
280  }
281  }
282 }
283 
284 Q_DECLARE_METATYPE( rec::rpc::MapInfo )
285 
286 #endif //_REC_RPC_ROSHELPER_H_
static REC_RPC_FUNCTION_IS_NOT_USED bool poseStampedToMap(tf::TransformListener *tf, const MapInfo &mapInfo, const geometry_msgs::PoseStamped &pose, QPointF *point, double *rotation_deg)
Definition: ROSHelper.h:148
MapInfo(float resolution_, float offsetX, float offsetY, std::string frame_id_="/map")
Definition: ROSHelper.h:62
#define REC_RPC_FUNCTION_IS_NOT_USED
Definition: defines.h:57
static REC_RPC_FUNCTION_IS_NOT_USED QPointF getPoint(const MapInfo &mapInfo, const tf::Pose &p)
Definition: ROSHelper.h:87
static REC_RPC_FUNCTION_IS_NOT_USED geometry_msgs::Pose toPose(const MapInfo &mapInfo, const QPointF &point, double rot_degrees)
Definition: ROSHelper.h:237
std::string frame_id
Definition: ROSHelper.h:84
static REC_RPC_FUNCTION_IS_NOT_USED float rad2deg(float rad)
Definition: rec_rpc_utils.h:44
static REC_RPC_FUNCTION_IS_NOT_USED bool toMap(tf::TransformListener *tf, const MapInfo &mapInfo, const geometry_msgs::Point &position, const std::string &sourceFrame, const ros::Time &time, QPointF *point)
Definition: ROSHelper.h:202
static REC_RPC_FUNCTION_IS_NOT_USED bool poseToMap(tf::TransformListener *tf, const MapInfo &mapInfo, const geometry_msgs::Pose &pose, const std::string &sourceFrame, const ros::Time &time, QPointF *point, double *rotation_deg)
Definition: ROSHelper.h:185
float offset[2]
Definition: ROSHelper.h:83
static REC_RPC_FUNCTION_IS_NOT_USED float deg2rad(float deg)
Definition: rec_rpc_utils.h:39
static REC_RPC_FUNCTION_IS_NOT_USED double getYaw(const tf::Pose &p)
Definition: ROSHelper.h:137
MapInfo(const nav_msgs::OccupancyGridConstPtr &msg, std::string frame_id_="/map")
Definition: ROSHelper.h:70
static REC_RPC_FUNCTION_IS_NOT_USED QImage fromOccupancyGrid(const nav_msgs::OccupancyGridConstPtr &msg)
Definition: ROSHelper.h:259
static REC_RPC_FUNCTION_IS_NOT_USED QVector< QPointF > getPoints(tf::TransformListener *tf, const MapInfo &mapInfo, const sensor_msgs::PointCloud &pointCloudIn)
Definition: ROSHelper.h:103
bool isEmpty() const
Definition: ROSHelper.h:78