From 670a75d3641e8012f144848a88f7db762a2faedf Mon Sep 17 00:00:00 2001 From: Rares Ambrus Date: Tue, 7 Jul 2015 18:22:33 +0200 Subject: [PATCH] Added WaypointTimestampService ObservationInstanceService and ObservationOctomapInstanceService --- semantic_map_publisher/CMakeLists.txt | 3 + .../include/semantic_map_publisher.h | 146 +++++++++++++++++- .../srv/ObservationInstanceService.srv | 6 + .../srv/ObservationOctomapInstanceService.srv | 6 + .../srv/WaypointTimestampService.srv | 3 + 5 files changed, 161 insertions(+), 3 deletions(-) create mode 100644 semantic_map_publisher/srv/ObservationInstanceService.srv create mode 100644 semantic_map_publisher/srv/ObservationOctomapInstanceService.srv create mode 100644 semantic_map_publisher/srv/WaypointTimestampService.srv diff --git a/semantic_map_publisher/CMakeLists.txt b/semantic_map_publisher/CMakeLists.txt index e93d038a..b45c6e30 100644 --- a/semantic_map_publisher/CMakeLists.txt +++ b/semantic_map_publisher/CMakeLists.txt @@ -22,7 +22,10 @@ add_service_files( MetaroomService.srv ObservationService.srv ObservationOctomapService.srv + ObservationInstanceService.srv + ObservationOctomapInstanceService.srv WaypointInfoService.srv + WaypointTimestampService.srv SensorOriginService.srv ) diff --git a/semantic_map_publisher/include/semantic_map_publisher.h b/semantic_map_publisher/include/semantic_map_publisher.h index e667c403..f6303bbf 100644 --- a/semantic_map_publisher/include/semantic_map_publisher.h +++ b/semantic_map_publisher/include/semantic_map_publisher.h @@ -24,8 +24,11 @@ // Services #include #include +#include #include +#include #include +#include #include @@ -55,14 +58,20 @@ class SemanticMapPublisher { typedef typename semantic_map_publisher::MetaroomService::Response MetaroomServiceResponse; typedef typename semantic_map_publisher::ObservationService::Request ObservationServiceRequest; typedef typename semantic_map_publisher::ObservationService::Response ObservationServiceResponse; + typedef typename semantic_map_publisher::ObservationInstanceService::Request ObservationInstanceServiceRequest; + typedef typename semantic_map_publisher::ObservationInstanceService::Response ObservationInstanceServiceResponse; typedef typename semantic_map_publisher::SensorOriginService::Request SensorOriginServiceRequest; typedef typename semantic_map_publisher::SensorOriginService::Response SensorOriginServiceResponse; // typedef typename semantic_map_publisher::DynamicClusterService::Request DynamicClusterServiceRequest; // typedef typename semantic_map_publisher::DynamicClusterService::Response DynamicClusterServiceResponse; typedef typename semantic_map_publisher::WaypointInfoService::Request WaypointInfoServiceRequest; typedef typename semantic_map_publisher::WaypointInfoService::Response WaypointInfoServiceResponse; + typedef typename semantic_map_publisher::WaypointTimestampService::Request WaypointTimestampServiceRequest; + typedef typename semantic_map_publisher::WaypointTimestampService::Response WaypointTimestampServiceResponse; typedef typename semantic_map_publisher::ObservationOctomapService::Request ObservationOctomapServiceRequest; typedef typename semantic_map_publisher::ObservationOctomapService::Response ObservationOctomapServiceResponse; + typedef typename semantic_map_publisher::ObservationOctomapInstanceService::Request ObservationOctomapInstanceServiceRequest; + typedef typename semantic_map_publisher::ObservationOctomapInstanceService::Response ObservationOctomapInstanceServiceResponse; struct ObsStruct { @@ -80,7 +89,10 @@ class SemanticMapPublisher { // bool dynamicClusterServiceCallback(DynamicClusterServiceRequest &req, DynamicClusterServiceResponse &res); bool observationServiceCallback(ObservationServiceRequest &req, ObservationServiceResponse &res); bool observationOctomapServiceCallback(ObservationOctomapServiceRequest &req, ObservationOctomapServiceResponse &res); + bool observationInstanceServiceCallback(ObservationInstanceServiceRequest &req, ObservationInstanceServiceResponse &res); + bool observationOctomapInstanceServiceCallback(ObservationOctomapInstanceServiceRequest &req, ObservationOctomapInstanceServiceResponse &res); bool waypointInfoServiceCallback(WaypointInfoServiceRequest &req, WaypointInfoServiceResponse &res); + bool waypointTimestampServiceCallback(WaypointTimestampServiceRequest &req, WaypointTimestampServiceResponse &res); bool sensorOriginServiceCallback(SensorOriginServiceRequest &req, SensorOriginServiceResponse &res); @@ -91,7 +103,10 @@ class SemanticMapPublisher { // ros::ServiceServer m_DynamicClusterServiceServer; ros::ServiceServer m_ObservationServiceServer; ros::ServiceServer m_ObservationOctomapServiceServer; + ros::ServiceServer m_ObservationInstanceServiceServer; + ros::ServiceServer m_ObservationOctomapInstanceServiceServer; ros::ServiceServer m_WaypointInfoServiceServer; + ros::ServiceServer m_WaypointTimestampServiceServer; ros::ServiceServer m_SensorOriginServiceServer; @@ -124,7 +139,10 @@ SemanticMapPublisher::SemanticMapPublisher(ros::NodeHandle nh) // m_DynamicClusterServiceServer = m_NodeHandle.advertiseService("SemanticMapPublisher/DynamicClusterService", &SemanticMapPublisher::dynamicClusterServiceCallback, this); m_ObservationServiceServer = m_NodeHandle.advertiseService("SemanticMapPublisher/ObservationService", &SemanticMapPublisher::observationServiceCallback, this); m_ObservationOctomapServiceServer = m_NodeHandle.advertiseService("SemanticMapPublisher/ObservationOctomapService", &SemanticMapPublisher::observationOctomapServiceCallback, this); + m_ObservationInstanceServiceServer = m_NodeHandle.advertiseService("SemanticMapPublisher/ObservationInstanceService", &SemanticMapPublisher::observationInstanceServiceCallback, this); + m_ObservationOctomapInstanceServiceServer = m_NodeHandle.advertiseService("SemanticMapPublisher/ObservationOctomapInstanceService", &SemanticMapPublisher::observationOctomapInstanceServiceCallback, this); m_WaypointInfoServiceServer= m_NodeHandle.advertiseService("SemanticMapPublisher/WaypointInfoService", &SemanticMapPublisher::waypointInfoServiceCallback, this); + m_WaypointTimestampServiceServer= m_NodeHandle.advertiseService("SemanticMapPublisher/WaypointTimestampService", &SemanticMapPublisher::waypointTimestampServiceCallback, this); m_SensorOriginServiceServer= m_NodeHandle.advertiseService("SemanticMapPublisher/SensorOriginService", &SemanticMapPublisher::sensorOriginServiceCallback, this); } @@ -154,7 +172,7 @@ bool SemanticMapPublisher::observationServiceCallback(ObservationServ ROS_INFO_STREAM("No observations for this waypoint "<::observationServiceCallback(ObservationServ } +template +bool SemanticMapPublisher::observationInstanceServiceCallback(ObservationInstanceServiceRequest &req, ObservationInstanceServiceResponse &res) +{ + using namespace std; + ROS_INFO_STREAM("Received an observation instance request for waypoint "< matchingObservations = semantic_map_load_utilties::getSweepXmlsForTopologicalWaypoint(m_dataFolder, req.waypoint_id); + if (matchingObservations.size() == 0) + { + ROS_INFO_STREAM("No observations for this waypoint "<(sweep_xml); + + CloudPtr observationCloud(new Cloud()); + pcl::VoxelGrid vg; + vg.setInputCloud (completeCloud); + vg.setLeafSize (req.resolution, req.resolution, req.resolution); + vg.filter (*observationCloud); + + sensor_msgs::PointCloud2 msg_observation; + pcl::toROSMsg(*observationCloud, msg_observation); + msg_observation.header.frame_id="/map"; + + res.cloud = msg_observation; + + // also publish on topic + m_PublisherObservation.publish(msg_observation); + + // get timestamp + auto sweep = SimpleXMLParser::loadRoomFromXML(sweep_xml, std::vector(), false); + res.observation_timestamp = boost::posix_time::to_simple_string(sweep.roomLogStartTime); + + ROS_INFO_STREAM("Published observation instance "< bool SemanticMapPublisher::waypointInfoServiceCallback(WaypointInfoServiceRequest &req, WaypointInfoServiceResponse &res) { @@ -225,6 +289,82 @@ bool SemanticMapPublisher::waypointInfoServiceCallback(WaypointInfoSe return true; } +template +bool SemanticMapPublisher::waypointTimestampServiceCallback(WaypointTimestampServiceRequest &req, WaypointTimestampServiceResponse &res) +{ + using namespace std; + ROS_INFO_STREAM("Received an observation request for waypoint "< matchingObservations = semantic_map_load_utilties::getSweepXmlsForTopologicalWaypoint(m_dataFolder, req.waypoint_id); + if (matchingObservations.size() == 0) + { + ROS_INFO_STREAM("No observations for this waypoint "<::loadRoomFromXML(obs, std::vector(), false); +// cout< +bool SemanticMapPublisher::observationOctomapInstanceServiceCallback(ObservationOctomapInstanceServiceRequest &req, ObservationOctomapInstanceServiceResponse &res) +{ + using namespace std; + ROS_INFO_STREAM("Received an observation octomap instance request for waypoint "< matchingObservations = semantic_map_load_utilties::getSweepXmlsForTopologicalWaypoint(m_dataFolder, req.waypoint_id); + if (matchingObservations.size() == 0) + { + ROS_INFO_STREAM("No observations for this waypoint "<(sweep_xml); + + sensor_msgs::PointCloud2 msg_observation; + pcl::toROSMsg(*completeCloud, msg_observation); + msg_observation.header.frame_id="/map"; + + Eigen::Vector4f centroid; + pcl::compute3DCentroid(*completeCloud, centroid); + octomap::point3d octo_centroid(centroid[0],centroid[1],centroid[2]); + // convert observation cloud to octomap and return it + octomap::Pointcloud oct_pc; + octomap::pointCloud2ToOctomap(msg_observation, oct_pc); + octomap::OcTree map(req.resolution); + map.insertPointCloud(oct_pc, octo_centroid); + octomap_msgs::Octomap octo_msg; + octomap_msgs::fullMapToMsg(map,octo_msg); + res.octomap = octo_msg; + + + // get timestamp + auto sweep = SimpleXMLParser::loadRoomFromXML(sweep_xml, std::vector(), false); + res.observation_timestamp = boost::posix_time::to_simple_string(sweep.roomLogStartTime); + + ROS_INFO_STREAM("Published observation octomap instance "< bool SemanticMapPublisher::observationOctomapServiceCallback(ObservationOctomapServiceRequest &req, ObservationOctomapServiceResponse &res) { @@ -237,7 +377,7 @@ bool SemanticMapPublisher::observationOctomapServiceCallback(Observat return true; } - sort(matchingObservations.begin(), matchingObservations.end()); +// sort(matchingObservations.begin(), matchingObservations.end()); reverse(matchingObservations.begin(), matchingObservations.end()); string latest = matchingObservations[0]; @@ -302,7 +442,7 @@ bool SemanticMapPublisher::sensorOriginServiceCallback(SensorOriginSe return true; } - sort(matchingObservations.begin(), matchingObservations.end()); +// sort(matchingObservations.begin(), matchingObservations.end()); reverse(matchingObservations.begin(), matchingObservations.end()); string latest = matchingObservations[0]; diff --git a/semantic_map_publisher/srv/ObservationInstanceService.srv b/semantic_map_publisher/srv/ObservationInstanceService.srv new file mode 100644 index 00000000..d832d7e6 --- /dev/null +++ b/semantic_map_publisher/srv/ObservationInstanceService.srv @@ -0,0 +1,6 @@ +string waypoint_id +int64 instance_number # convention 0 - oldest available +float64 resolution +--- +sensor_msgs/PointCloud2 cloud +string observation_timestamp diff --git a/semantic_map_publisher/srv/ObservationOctomapInstanceService.srv b/semantic_map_publisher/srv/ObservationOctomapInstanceService.srv new file mode 100644 index 00000000..ab4851be --- /dev/null +++ b/semantic_map_publisher/srv/ObservationOctomapInstanceService.srv @@ -0,0 +1,6 @@ +string waypoint_id +int64 instance_number # convention 0 - oldest available +float64 resolution +--- +octomap_msgs/Octomap octomap +string observation_timestamp diff --git a/semantic_map_publisher/srv/WaypointTimestampService.srv b/semantic_map_publisher/srv/WaypointTimestampService.srv new file mode 100644 index 00000000..24c81e5f --- /dev/null +++ b/semantic_map_publisher/srv/WaypointTimestampService.srv @@ -0,0 +1,3 @@ +string waypoint_id +--- +string[] waypoint_timestamps