bool
WmMapServer::mapSrv(watermellon::GetMap::Request &req, watermellon::GetMap::Response &res)
{
ROS_INFO("Sending map on service request");
if (map_->size() <= 1){
ROS_WARN("Nothing to publish, map is empty");
return false;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr map_temp(new pcl::PointCloud<pcl::PointXYZRGB>);
tf::Transform Odom2Map;
Odom2Map.setOrigin(tf::Vector3(origin_.position.x, origin_.position.y, origin_.position.z));
Odom2Map.setRotation(tf::Quaternion(origin_.orientation.x, origin_.orientation.y, origin_.orientation.z, origin_.orientation.w));
Eigen::Matrix4f Odom2Maptf;
pcl_ros::transformAsMatrix(Odom2Map.inverse(), Odom2Maptf);
pcl::transformPointCloud(*map_, *map_temp, Odom2Maptf);
pcl::toROSMsg (*map_temp, res.map);
res.map.header.frame_id = worldFrameId_;
res.map.header.stamp = ros::Time::now();
return true;
}
void data_write_temp(uint16_t tempreading)
{
//data_write_buffer_str("Temperature in degº");globalhpos=0;
globalhpos=0;
int current_range_min;
int current_range_max;
if(print_temp_flag==1)
{
current_range_max=temp_max;
current_range_min=temp_min;
}
else
{
current_range_max=range_max;
current_range_min=range_min;
}
//tackle problem of combining axis and label
long int temp_pos=map_temp(tempreading,current_range_min,current_range_max,17,383);
for(int i=0;i<48;i++)
buffer[0][i]=0b00000000; //set all other bits to 0
if(vertical_buffer_pointer<vertical_buffer_length)
{
buffer[0][0]=vertical_buffer[vertical_buffer_pointer][0];
buffer[0][1]=vertical_buffer[vertical_buffer_pointer][1];
vertical_buffer_pointer++;
}
buffer[0][2]=0b10000000; //put the dot for the x axis in the first bit of the third byte
uint8_t byte_pos=temp_pos/8;
uint8_t shift_point=(7-temp_pos%8);
char thisbyte=(1<<shift_point);
buffer[0][byte_pos]=thisbyte;
}
void WmMapServer::publishMap(const ros::Time& rostime){
ros::WallTime startTime = ros::WallTime::now();
size_t mapSize = map_->size();
if (mapSize <= 1){
ROS_WARN("Nothing to publish, octree is empty");
return;
}
ROS_DEBUG("Map points %ld ", map_->size());
if (pointCloudPub_.getNumSubscribers() > 0){
pcl::PointCloud<pcl::PointXYZRGB>::Ptr map_temp(new pcl::PointCloud<pcl::PointXYZRGB>);
tf::Transform Odom2Map;
Odom2Map.setOrigin(tf::Vector3(origin_.position.x, origin_.position.y, origin_.position.z));
Odom2Map.setRotation(tf::Quaternion(origin_.orientation.x, origin_.orientation.y, origin_.orientation.z, origin_.orientation.w));
Eigen::Matrix4f Odom2Maptf;
pcl_ros::transformAsMatrix(Odom2Map.inverse(), Odom2Maptf);
pcl::transformPointCloud(*map_, *map_temp, Odom2Maptf);
sensor_msgs::PointCloud2 cloud;
pcl::toROSMsg (*map_temp, cloud);
cloud.header.frame_id = worldFrameId_;
cloud.header.stamp = rostime;
pointCloudPub_.publish(cloud);
}
double total_elapsed = (ros::WallTime::now() - startTime).toSec();
ROS_DEBUG("Map publishing in OctomapServer took %f sec", total_elapsed);
}
