Permalink
Browse files

get changed points service

  • Loading branch information...
SebastianInd committed Feb 26, 2018
1 parent f3f58a6 commit ef72e6d9b1dac6c451a2bcb19476bb3ff1272997
@@ -42,6 +42,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <volumetric_msgs/SaveMap.h>
#include <volumetric_msgs/SetBoxOccupancy.h>
#include <volumetric_msgs/SetDisplayBounds.h>
#include <volumetric_msgs/GetChangedPoints.h>
#include <pcl_conversions/pcl_conversions.h>
@@ -94,6 +95,10 @@ class OctomapManager : public OctomapWorld {
volumetric_msgs::SetDisplayBounds::Request& request,
volumetric_msgs::SetDisplayBounds::Response& response);
bool getChangedPointsCallback(
volumetric_msgs::GetChangedPoints::Request& request,
volumetric_msgs::GetChangedPoints::Response& response);
void transformCallback(const geometry_msgs::TransformStamped& transform_msg);
private:
@@ -167,6 +172,7 @@ class OctomapManager : public OctomapWorld {
ros::ServiceServer save_point_cloud_service_;
ros::ServiceServer set_box_occupancy_service_;
ros::ServiceServer set_display_bounds_service_;
ros::ServiceServer get_changed_points_service_;
// Keep state of the cameras.
sensor_msgs::CameraInfoPtr left_info_;
@@ -214,6 +214,13 @@ void OctomapManager::advertiseServices() {
"set_box_occupancy", &OctomapManager::setBoxOccupancyCallback, this);
set_display_bounds_service_ = nh_private_.advertiseService(
"set_display_bounds", &OctomapManager::setDisplayBoundsCallback, this);
bool change_detection_enabled;
nh_private_.param("change_detection_enabled", change_detection_enabled,
false);
if (change_detection_enabled) {
get_changed_points_service_ = nh_.advertiseService(
"get_changed_points", &OctomapManager::getChangedPointsCallback, this);
}
}
void OctomapManager::advertisePublishers() {
@@ -383,6 +390,28 @@ bool OctomapManager::setDisplayBoundsCallback(
return true;
}
bool OctomapManager::getChangedPointsCallback(
volumetric_msgs::GetChangedPoints::Request& request,
volumetric_msgs::GetChangedPoints::Response& response) {
std::vector<Eigen::Vector3d> changed_points;
std::vector<bool> changed_states;
getChangedPoints(&changed_points, &changed_states);
if (changed_points.size() != changed_states.size()) {
std::cerr << "In getChangedPointsCallback changed_points and "
"changed_states have different size!\n";
return false;
}
int size = changed_points.size();
response.size = size;
response.changed_points.resize(size);
response.changed_states.resize(size);
for (int i = 0; i < size; ++i) {
tf::vectorKindrToMsg(changed_points[i], &(response.changed_points[i]));
response.changed_states[i] = changed_states[i];
}
return true;
}
void OctomapManager::leftCameraInfoCallback(
const sensor_msgs::CameraInfoPtr& left_info) {
left_info_ = left_info;
@@ -0,0 +1,4 @@
---
int32 size
geometry_msgs/Vector3[] changed_points
bool[] changed_states

0 comments on commit ef72e6d

Please sign in to comment.