void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud& cloud){
Stamped<btVector3> global_origin;
//create a new observation on the list to be populated
observation_list_.push_front(Observation());
//check whether the origin frame has been set explicitly or whether we should get it from the cloud
string origin_frame = sensor_frame_ == "" ? cloud.header.frame_id : sensor_frame_;
try{
//given these observations come from sensors... we'll need to store the origin pt of the sensor
Stamped<btVector3> local_origin(btVector3(0, 0, 0), cloud.header.stamp, origin_frame);
tf_.transformPoint(global_frame_, local_origin, global_origin);
observation_list_.front().origin_.x = global_origin.getX();
observation_list_.front().origin_.y = global_origin.getY();
observation_list_.front().origin_.z = global_origin.getZ();
//make sure to pass on the raytrace/obstacle range of the observation buffer to the observations the costmap will see
observation_list_.front().raytrace_range_ = raytrace_range_;
observation_list_.front().obstacle_range_ = obstacle_range_;
sensor_msgs::PointCloud global_frame_cloud;
//transform the point cloud
tf_.transformPointCloud(global_frame_, cloud, global_frame_cloud);
global_frame_cloud.header.stamp = cloud.header.stamp;
//now we need to remove observations from the cloud that are below or above our height thresholds
sensor_msgs::PointCloud& observation_cloud = observation_list_.front().cloud_;
unsigned int cloud_size = global_frame_cloud.points.size();
observation_cloud.points.resize(cloud_size);
unsigned int point_count = 0;
//copy over the points that are within our height bounds
for(unsigned int i = 0; i < cloud_size; ++i){
if(global_frame_cloud.points[i].z <= max_obstacle_height_ && global_frame_cloud.points[i].z >= min_obstacle_height_){
observation_cloud.points[point_count++] = global_frame_cloud.points[i];
}
}
//resize the cloud for the number of legal points
observation_cloud.points.resize(point_count);
observation_cloud.header.stamp = cloud.header.stamp;
observation_cloud.header.frame_id = global_frame_cloud.header.frame_id;
}
catch(TransformException& ex){
//if an exception occurs, we need to remove the empty observation from the list
observation_list_.pop_front();
ROS_ERROR("TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s", sensor_frame_.c_str(),
cloud.header.frame_id.c_str(), ex.what());
return;
}
//if the update was successful, we want to update the last updated time
last_updated_ = ros::Time::now();
//we'll also remove any stale observations from the list
purgeStaleObservations();
}
/**
Traduit en coordonnees locales un mouvement / vecteur initialement
exprime en coordonnees du parent.
@param movement Vecteur exprime en coordonnees du parent
@return le meme vecteur, exprime en coordonnees locales
*/
QPointF DiagramTextItem::mapMovementFromParent(const QPointF &movement) const {
// on definit deux points sur le parent
QPointF parent_origin(0.0, 0.0);
QPointF parent_movement_point(movement);
// on les mappe sur ce QGraphicsItem
QPointF local_origin(mapFromParent(parent_origin));
QPointF local_movement_point(mapFromParent(parent_movement_point));
// on calcule le vecteur represente par ces deux points
return(local_movement_point - local_origin);
}
/**
Traduit en coordonnees de l'item parent un mouvement / vecteur initialement
exprime en coordonnees locales.
@param movement Vecteur exprime en coordonnees locales
@return le meme vecteur, exprime en coordonnees du parent
*/
QPointF DiagramTextItem::mapMovementToParent(const QPointF &movement) const {
// on definit deux points en coordonnees locales
QPointF local_origin(0.0, 0.0);
QPointF local_movement_point(movement);
// on les mappe sur la scene
QPointF parent_origin(mapToParent(local_origin));
QPointF parent_movement_point(mapToParent(local_movement_point));
// on calcule le vecteur represente par ces deux points
return(parent_movement_point - parent_origin);
}
