27 #ifndef _REC_RPC_ROSHELPER_H_ 28 #define _REC_RPC_ROSHELPER_H_ 31 #include "rec/rpc/utils.h" 41 #include <tf/transform_listener.h> 42 #include <geometry_msgs/Point.h> 43 #include <sensor_msgs/PointCloud.h> 45 #include <nav_msgs/OccupancyGrid.h> 62 MapInfo(
float resolution_,
float offsetX,
float offsetY, std::string frame_id_ =
"/map" )
70 MapInfo(
const nav_msgs::OccupancyGridConstPtr& msg, std::string frame_id_ =
"/map" )
73 offset[0] = -((int)msg->info.width) - (msg->info.origin.position.x /
resolution);
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] );
105 QVector< QPointF > points;
109 tf::StampedTransform transform;
110 tf->lookupTransform( mapInfo.
frame_id, pointCloudIn.header.frame_id, ros::Time(0), transform );
112 points.resize( pointCloudIn.points.size() );
114 for(
unsigned int i=0; i<pointCloudIn.points.size(); ++i )
117 p.setOrigin( btVector3( pointCloudIn.points[i].x, pointCloudIn.points[i].y, 0 ) );
124 catch(tf::LookupException& ex) {
125 ROS_ERROR(
"No Transform available Error: %s\n", ex.what());
127 catch(tf::ConnectivityException& ex) {
128 ROS_ERROR(
"Connectivity Error: %s\n", ex.what());
130 catch(tf::ExtrapolationException& ex) {
131 ROS_ERROR(
"Extrapolation Error: %s\n", ex.what());
139 btScalar useless_pitch, useless_roll, yaw;
140 p.getBasis().getRPY( useless_roll, useless_pitch, yaw );
142 double x = cos( yaw );
143 double y = sin( yaw );
145 return rad2deg( atan2( y, -x ) );
152 tf::StampedTransform transform;
153 tf->lookupTransform( mapInfo.
frame_id, pose.header.frame_id, ros::Time(0), transform );
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 ) );
163 *rotation_deg =
getYaw( p );
169 catch(tf::LookupException& ex) {
170 ROS_ERROR(
"No Transform available Error: %s\n", ex.what());
173 catch(tf::ConnectivityException& ex) {
174 ROS_ERROR(
"Connectivity Error: %s\n", ex.what());
177 catch(tf::ExtrapolationException& ex) {
178 ROS_ERROR(
"Extrapolation Error: %s\n", ex.what());
187 geometry_msgs::PoseStamped poseStamped;
188 poseStamped.header.frame_id = sourceFrame;
189 poseStamped.header.stamp = time;
190 poseStamped.pose = pose;
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;
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 )
222 geometry_msgs::PoseStamped poseStamped;
223 poseStamped.header.frame_id = sourceFrame;
224 poseStamped.header.stamp = time;
225 poseStamped.pose = pose;
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] ) );
244 double rot =
deg2rad( rot_degrees );
245 double x = cos( rot );
246 double y = sin( rot );
247 rot = atan2( y, -x );
249 tf::Quaternion q = tf::createQuaternionFromYaw( rot );
251 pose.orientation.x = q.x();
252 pose.orientation.y = q.y();
253 pose.orientation.z = q.z();
254 pose.orientation.w = q.w();
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;
265 for( i = 0; i < 101; ++i )
267 QRgb col = 0xa0000000;
268 int val = (100 - i) / 100 * 255;
269 col |= val | (val << 8) | (val << 16);
270 colorTable.push_back(col);
272 for( ; i < 255; ++i )
274 colorTable.push_back( 0xFFFF0000 );
276 colorTable.push_back( 0xFF888888 );
277 imgBuffer.setColorTable( colorTable );
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)
MapInfo(float resolution_, float offsetX, float offsetY, std::string frame_id_="/map")
#define REC_RPC_FUNCTION_IS_NOT_USED
static REC_RPC_FUNCTION_IS_NOT_USED QPointF getPoint(const MapInfo &mapInfo, const tf::Pose &p)
static REC_RPC_FUNCTION_IS_NOT_USED geometry_msgs::Pose toPose(const MapInfo &mapInfo, const QPointF &point, double rot_degrees)
static REC_RPC_FUNCTION_IS_NOT_USED float rad2deg(float rad)
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)
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)
static REC_RPC_FUNCTION_IS_NOT_USED float deg2rad(float deg)
static REC_RPC_FUNCTION_IS_NOT_USED double getYaw(const tf::Pose &p)
MapInfo(const nav_msgs::OccupancyGridConstPtr &msg, std::string frame_id_="/map")
static REC_RPC_FUNCTION_IS_NOT_USED QImage fromOccupancyGrid(const nav_msgs::OccupancyGridConstPtr &msg)
static REC_RPC_FUNCTION_IS_NOT_USED QVector< QPointF > getPoints(tf::TransformListener *tf, const MapInfo &mapInfo, const sensor_msgs::PointCloud &pointCloudIn)