Skip to content
Permalink
Browse files
initial port, compiles
  • Loading branch information
mikeferguson committed Aug 28, 2020
1 parent 03df6fe commit ae0ea1e22ece7eb17b6c2794408f9825f7284f8d
Show file tree
Hide file tree
Showing 5 changed files with 236 additions and 220 deletions.
@@ -2,26 +2,51 @@ cmake_minimum_required(VERSION 3.5)
project(ubr1_navigation)

find_package(ament_cmake REQUIRED)
find_package(nav2_costmap_2d REQUIRED)
find_package(pluginlib REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(OpenCV REQUIRED)

install(
DIRECTORY config
DESTINATION share/${PROJECT_NAME}/
include_directories(
include
${OpenCV_INCLUDE_DIRS}
)

add_library(depth_layer SHARED
src/depth_layer.cpp
)
ament_target_dependencies(depth_layer
nav2_costmap_2d
pluginlib
sensor_msgs
tf2_ros
)
target_link_libraries(depth_layer
${OpenCV_LIBRARIES}
)

install(
DIRECTORY launch
DIRECTORY config launch maps
DESTINATION share/${PROJECT_NAME}/
)

install(
DIRECTORY maps
DESTINATION share/${PROJECT_NAME}/
FILES costmap_plugins.xml
DESTINATION share/${PROJECT_NAME}
)

install(
PROGRAMS
scripts/tilt_head.py
PROGRAMS scripts/tilt_head.py
DESTINATION lib/${PROJECT_NAME}
)

install(
TARGETS depth_layer
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
)

pluginlib_export_plugin_description_file(nav2_costmap_2d costmap_plugins.xml)
ament_package()
@@ -0,0 +1,7 @@
<class_libraries>
<library path="depth_layer">
<class type="nav2_costmap_2d::DepthLayer" base_class_type="nav2_costmap_2d::Layer">
<description>A costmap layer that extracts ground plane and clears it.</description>
</class>
</library>
</class_libraries>
@@ -1,4 +1,5 @@
/*
* Copyright (c) 2020, Michael Ferguson
* Copyright (c) 2015-2016, Fetch Robotics Inc.
* All rights reserved.
*
@@ -28,57 +29,58 @@

// Author: Anuj Pasricha, Michael Ferguson

#ifndef FETCH_DEPTH_LAYER_DEPTH_LAYER_H
#define FETCH_DEPTH_LAYER_DEPTH_LAYER_H
#ifndef UBR1_NAVIGATION__DEPTH_LAYER_HPP_
#define UBR1_NAVIGATION__DEPTH_LAYER_HPP_

#include <boost/thread/mutex.hpp>
#include <boost/shared_ptr.hpp>
#include <costmap_2d/voxel_layer.h>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/image_encodings.h>
#include <tf2_ros/message_filter.h>
#include <message_filters/subscriber.h>
#include <mutex>

#include "nav2_costmap_2d/observation_buffer.hpp"
#include "nav2_costmap_2d/voxel_layer.hpp"
#include "cv_bridge/cv_bridge.h"
#include "image_transport/image_transport.hpp"
#include "sensor_msgs/image_encodings.hpp"
#include "sensor_msgs/msg/image.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include "tf2_ros/message_filter.h"
//#include <message_filters/subscriber.h>

#include <opencv2/rgbd.hpp>
using cv::rgbd::DepthCleaner;
using cv::rgbd::RgbdNormals;
using cv::rgbd::RgbdPlane;
using cv::rgbd::depthTo3d;

namespace costmap_2d
namespace nav2_costmap_2d
{

/**
* @class FetchDepthLayer
* @class DepthLayer
* @brief A costmap layer that extracts ground plane and clears it.
*/
class FetchDepthLayer : public VoxelLayer
class DepthLayer : public VoxelLayer
{
public:
/**
* @brief Constructor
*/
FetchDepthLayer();
DepthLayer();

/**
* @brief Destructor for the depth costmap layer
*/
virtual ~FetchDepthLayer();
virtual ~DepthLayer();

/**
* @brief Initialization function for the DepthLayer
*/
virtual void onInitialize();

private:
void cameraInfoCallback(
const sensor_msgs::CameraInfo::ConstPtr& msg);
void depthImageCallback(
const sensor_msgs::Image::ConstPtr& msg);
void cameraInfoCallback(const sensor_msgs::msg::CameraInfo::SharedPtr msg);
void depthImageCallback(sensor_msgs::msg::Image::ConstSharedPtr msg);

boost::shared_ptr<costmap_2d::ObservationBuffer> marking_buf_;
boost::shared_ptr<costmap_2d::ObservationBuffer> clearing_buf_;
std::shared_ptr<nav2_costmap_2d::ObservationBuffer> marking_buf_;
std::shared_ptr<nav2_costmap_2d::ObservationBuffer> clearing_buf_;

// should we publish the marking/clearing observations
bool publish_observations_;
@@ -87,9 +89,6 @@ class FetchDepthLayer : public VoxelLayer
// something is considered an obstacle
double observations_threshold_;

// should we dynamically find the ground plane?
bool find_ground_plane_;

// if finding ground plane, limit the tilt
// with respect to base_link frame
double ground_threshold_;
@@ -108,21 +107,21 @@ class FetchDepthLayer : public VoxelLayer

// retrieves depth image from head_camera
// used to fit ground plane to
boost::shared_ptr< message_filters::Subscriber<sensor_msgs::Image> > depth_image_sub_;
boost::shared_ptr< tf2_ros::MessageFilter<sensor_msgs::Image> > depth_image_filter_;
std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::Image>> depth_image_sub_;
std::shared_ptr<tf2_ros::MessageFilter<sensor_msgs::msg::Image>> depth_image_filter_;

// retrieves camera matrix for head_camera
// used in calculating ground plane
ros::Subscriber camera_info_sub_;
rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr camera_info_sub_;

// publishes clearing observations
ros::Publisher clearing_pub_;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr clearing_pub_;

// publishes marking observations
ros::Publisher marking_pub_;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr marking_pub_;

// camera intrinsics
boost::mutex mutex_K_;
std::mutex mutex_K_;
cv::Mat K_;

// clean the depth image
@@ -133,7 +132,6 @@ class FetchDepthLayer : public VoxelLayer
cv::Ptr<RgbdPlane> plane_estimator_;
};

} // namespace costmap_2d

#endif // FETCH_DEPTH_LAYER_DEPTH_LAYER_H
} // namespace nav2_costmap_2d

#endif // UBR1_NAVIGATION__DEPTH_LAYER_HPP
@@ -9,6 +9,10 @@
<author>Michael Ferguson</author>

<buildtool_depend>ament_cmake</buildtool_depend>
<depend>nav2_costmap_2d</depend>
<depend>pluginlib</depend>
<depend>sensor_msgs</depend>
<depend>tf2_ros</depend>
<exec_depend>slam_toolbox</exec_depend>

<export>

0 comments on commit ae0ea1e

Please sign in to comment.