Costmap

CMakeLists.txt

@@ -180,6 +180,10 @@ endif ()
 ## Perception
 
 mrover_add_nodelet(tag_detector src/perception/tag_detector/*.cpp src/perception/tag_detector src/perception/tag_detector/pch.hpp)
+mrover_nodelet_link_libraries(tag_detector opencv_core opencv_objdetect opencv_aruco opencv_imgproc tbb lie)
+
+mrover_add_nodelet(costmap src/perception/cost_map/*.cpp src/perception/cost_map src/perception/cost_map/pch.hpp)
+mrover_nodelet_link_libraries(costmap lie)
 mrover_nodelet_link_libraries(tag_detector opencv_core opencv_objdetect opencv_aruco opencv_imgproc opencv_highgui lie)
 
 # if (CUDA_FOUND)

package.xml

@@ -82,6 +82,7 @@
   <export>
     <nodelet plugin="${prefix}/plugins/tag_detector_plugin.xml" />
     <nodelet plugin="${prefix}/plugins/zed_plugin.xml" />
+    <nodelet plugin="${prefix}/plugins/costmap.xml" />
     <nodelet plugin="${prefix}/plugins/can.xml" />
     <nodelet plugin="${prefix}/plugins/simulator.xml" />
   </export>

plugins/costmap.xml

@@ -0,0 +1,6 @@
+<library path="lib/libcostmap_nodelet">
+    <class name="mrover/CostMapNodelet"
+           type="mrover::CostMapNodelet"
+           base_class_type="nodelet::Nodelet">
+    </class>
+</library>

src/perception/cost_map/README.md

@@ -0,0 +1,21 @@
+(WIP)
+
+### Code Layout
+
+- [cost_map.cpp](./cost_map.cpp) Mainly ROS node setup (topics, parameters, etc.)
+- [cost_map.processing.cpp](./cost_map.processing.cpp) Processes ~~pointclouds~~ occupancy grids to continuously update the global terrain occupancy grid
+
+### Explanation
+
+The goal of this code is to take in a local occupancy grid / costmap (using http://docs.ros.org/en/melodic/api/nav_msgs/html/msg/OccupancyGrid.html) and output a global occupancy grid (again using http://docs.ros.org/en/melodic/api/nav_msgs/html/msg/OccupancyGrid.html).
+
+~~The goal of this code is to take in pointcloud data from the zed and output a cost map / occupancy grid that represents the difficulty of passing terrain.~~ 
+
+When a new local occupancy grid is received, the nodelet transforms it into the global frame using tf_tree information about the rover's frame relative to a chosen origin. The resolution (size of each grid space in physical units), grid width (in physical units), and grid height (in physical units) is used to determine the size of the costmap and to which grid space a given cost point is used to update. For simplicity, the new grid space's cost is equal to the maximum cost among the point cloud points that fall within the grid space. The cost map is then published for use by navigation nodelets.
+
+~~When a new pointcloud is received, the nodelet first calculates the costs associated with each point in the pointcloud. The cost at a particular location is 100 if the curvature estimate is above our threshold, 0 if it is not, or -1 if there is no data for that location. This discreteness is helpful for the navigation functions using this costmap, as it minimizes erratic movement to account for new data.~~
+
+~~After costs are calculated, each point's location is transformed from the local basis (relative to the rover) to the global basis (relative to the starting location).~~
+
+~~Processed cost point data is then used to update the rover's costmap. The resolution (size of each grid space in physical units), grid width (in physical units), and grid height (in physical units) is used to determine the size of the costmap and to which grid space a given cost point is used to update. For simplicity, the new grid space's cost is equal to the maximum cost among the point cloud points that fall within the grid space. The cost map is then published for use by navigation nodelets.~~
+

src/perception/cost_map/cost_map.cpp

@@ -0,0 +1,37 @@
+#include "cost_map.hpp"
+
+namespace mrover {
+
+    void CostMapNodelet::onInit() {
+        mNh = getMTNodeHandle();
+        mPnh = getMTPrivateNodeHandle(); // Unused for now
+        mNh.param<bool>("publish_cost_map", mPublishCostMap, true);
+        mNh.param<float>("resolution", mResolution, 0.5);
+        mNh.param<float>("map_width", mDimension, 60);
+
+        mCostMapPub = mCmt.advertise<nav_msgs::OccupancyGrid>("costmap", 1); // We publish our results to "costmap"
+
+        mPcSub = mNh.subscribe("camera/left/points", 1, &CostMapNodelet::pointCloudCallback, this);
+
+        mGlobalGridMsg.info.resolution = mResolution;
+        mGlobalGridMsg.info.width = (int) (mDimension / mResolution);
+        mGlobalGridMsg.info.height = (int) (mDimension / mResolution);
+        // make center of map at (0, 0)
+        mGlobalGridMsg.info.origin.position.x = -1 * mDimension / 2;
+        mGlobalGridMsg.info.origin.position.y = -1 * mDimension / 2;
+
+        mGlobalGridMsg.data.resize(mGlobalGridMsg.info.width * mGlobalGridMsg.info.height);
+        std::fill(mGlobalGridMsg.data.begin(), mGlobalGridMsg.data.end(), -1);
+    }
+} // namespace mrover
+
+/*
+TODO:
+- Nodelet takes in pointcloud
+PARALLELIZE
+- Get cost of point
+- Transform point to nav basis
+- Stitch to nav grid
+END
+
+*/

src/perception/cost_map/cost_map.hpp

@@ -0,0 +1,32 @@
+#include "pch.hpp"
+
+namespace mrover {
+
+
+    class CostMapNodelet : public nodelet::Nodelet {
+    private:
+        ros::NodeHandle mNh, mPnh, mCmt;
+        ros::Publisher mCostMapPub;
+        ros::Subscriber mPcSub;
+        void onInit() override;
+
+        bool mPublishCostMap{}; // If set, publish the global costmap
+        float mResolution{};    // Meters per cell
+        float mDimension{};     // Dimensions of the square costmap in meters
+        float mNormalThreshold = 0.9;
+        tf2_ros::Buffer tf_buffer;
+        tf2_ros::TransformListener tf_listener{tf_buffer};
+
+        std::optional<SE3> mPreviousPose;
+        nav_msgs::OccupancyGrid mGlobalGridMsg;
+
+
+    public:
+        CostMapNodelet() = default;
+
+        ~CostMapNodelet() override = default;
+
+        void configCallback();
+        void pointCloudCallback(sensor_msgs::PointCloud2ConstPtr const& msg);
+    };
+} // namespace mrover

src/perception/cost_map/cost_map.processing.cpp

@@ -0,0 +1,62 @@
+#include "cost_map.hpp"
+#include "../point.hpp"
+
+#include "loop_profiler.hpp"
+#include <Eigen/src/Core/Matrix.h>
+#include <Eigen/src/Core/util/ForwardDeclarations.h>
+#include <cstdint>
+#include <pstl/glue_execution_defs.h>
+#include <regex>
+
+namespace mrover {
+
+    void CostMapNodelet::pointCloudCallback(sensor_msgs::PointCloud2ConstPtr const& msg) {
+        assert(msg);
+        assert(msg->height > 0);
+        assert(msg->width > 0);
+
+        if (!mPublishCostMap) return;
+        try {
+            SE3 zed_to_map = SE3::fromTfTree(tf_buffer, "map", "zed2i_left_camera_frame");
+            auto* points = reinterpret_cast<Point const*>(msg->data.data());
+            int step = 4;
+            uint32_t num_points = msg->width * msg->height / step;
+            Eigen::MatrixXd point_matrix(4, num_points);
+            Eigen::MatrixXd normal_matrix(4, num_points);
+            // std::for_each(points, points + msg->width * msg->height, [&](auto* point) {
+            for (auto point = points; point - points < msg->width * msg->height; point += step) {
+                Eigen::Vector4d p{point->x, point->y, 0, 1};
+                point_matrix.col((point - points) / step) = p;
+                Eigen::Vector4d normal{point->normal_x, point->normal_y, point->normal_z, 0};
+                normal_matrix.col((point - points) / step) = normal;
+            }
+
+            point_matrix = zed_to_map.matrix() * point_matrix;
+            normal_matrix = zed_to_map.matrix() * normal_matrix;
+
+            for (uint32_t i = 0; i < num_points; i++) {
+                double x = point_matrix(0, i);
+                double y = point_matrix(1, i);
+                Eigen::Vector4d n = normal_matrix.col(i);
+
+                if (x >= -1 * mDimension / 2 && x <= mDimension / 2 &&
+                    y >= -1 * mDimension / 2 && y <= mDimension / 2) {
+                    int x_index = floor((x - mGlobalGridMsg.info.origin.position.x) / mGlobalGridMsg.info.resolution);
+                    int y_index = floor((y - mGlobalGridMsg.info.origin.position.y) / mGlobalGridMsg.info.resolution);
+                    auto i = mGlobalGridMsg.info.width * y_index + x_index;
+
+                    Eigen::Vector3d normal{n.x(), n.y(), n.z()};
+                    normal.normalize();
+                    // get vertical component of (unit) normal vector
+                    double z_comp = normal.z();
+                    // small z component indicates largely horizontal normal (surface is vertical)
+                    signed char cost = z_comp < mNormalThreshold ? 100 : 0;
+
+                    mGlobalGridMsg.data[i] = std::max(mGlobalGridMsg.data[i], cost);
+                }
+            }
+            mCostMapPub.publish(mGlobalGridMsg);
+        } catch (...) {
+        }
+    }
+} // namespace mrover

src/perception/cost_map/main.cpp

@@ -0,0 +1,22 @@
+#include "cost_map.hpp"
+
+#ifdef MROVER_IS_NODELET
+
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(mrover::CostMapNodelet, nodelet::Nodelet)
+
+#else
+
+int main(int argc, char** argv) {
+    ros::init(argc, argv, "costmap");
+
+    // Start Costmap Nodelet
+    nodelet::Loader nodelet;
+    nodelet.load(ros::this_node::getName(), "mrover/CostMapNodelet", ros::names::getRemappings(), {});
+
+    ros::spin();
+
+    return EXIT_SUCCESS;
+}
+
+#endif

src/perception/cost_map/pch.hpp

@@ -0,0 +1,16 @@
+#pragma once
+
+#include <algorithm>
+#include <cstdint>
+
+#include <nav_msgs/OccupancyGrid.h>
+#include <nodelet/loader.h>
+#include <nodelet/nodelet.h>
+#include <ros/node_handle.h>
+#include <ros/publisher.h>
+#include <se3.hpp>
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/point_cloud2_iterator.h>
+#include <tf2_ros/transform_broadcaster.h>
+
+#include <mrover/DetectorParamsConfig.h>

src/perception/zed_wrapper/zed_wrapper.bridge.cu

@@ -11,7 +11,7 @@ namespace mrover {
     /**
      * @brief Runs on the GPU, interleaving the XYZ and BGRA buffers into a single buffer of #Point structs.
      */
-    __global__ void fillPointCloudMessageKernel(sl::float4* xyzGpuPtr, sl::uchar4* bgraGpuPtr, Point* pcGpuPtr, size_t size) {
+    __global__ void fillPointCloudMessageKernel(sl::float4* xyzGpuPtr, sl::uchar4* bgraGpuPtr, sl::float4* normalsGpuPtr, Point* pcGpuPtr, size_t size) {
         // This function is invoked once per element at index #i in the point cloud
         size_t i = blockIdx.x * blockDim.x + threadIdx.x;
         if (i >= size) return;
@@ -23,6 +23,10 @@ namespace mrover {
         pcGpuPtr[i].g = bgraGpuPtr[i].g;
         pcGpuPtr[i].r = bgraGpuPtr[i].b;
         pcGpuPtr[i].a = bgraGpuPtr[i].a;
+        pcGpuPtr[i].normal_x = normalsGpuPtr[i].x;
+        pcGpuPtr[i].normal_x = normalsGpuPtr[i].y;
+        pcGpuPtr[i].normal_x = normalsGpuPtr[i].z;
+        // ROS_WARN(xyzGpuPtr[i].x);
     }
 
     /**
@@ -33,7 +37,7 @@ namespace mrover {
      * @param pcGpu     Point cloud buffer on the GPU (@see Point)
      * @param msg       Point cloud message with buffer on the CPU
      */
-    void fillPointCloudMessageFromGpu(sl::Mat& xyzGpu, sl::Mat& bgraGpu, PointCloudGpu& pcGpu, sensor_msgs::PointCloud2Ptr const& msg) {
+    void fillPointCloudMessageFromGpu(sl::Mat& xyzGpu, sl::Mat& bgraGpu, sl::Mat& normalsGpu, PointCloudGpu& pcGpu, sensor_msgs::PointCloud2Ptr const& msg) {
         assert(bgraGpu.getWidth() >= xyzGpu.getWidth());
         assert(bgraGpu.getHeight() >= xyzGpu.getHeight());
         assert(bgraGpu.getChannels() == 4);
@@ -42,6 +46,7 @@ namespace mrover {
 
         auto* bgraGpuPtr = bgraGpu.getPtr<sl::uchar4>(sl::MEM::GPU);
         auto* xyzGpuPtr = xyzGpu.getPtr<sl::float4>(sl::MEM::GPU);
+        auto* normalsGpuPtr = normalsGpu.getPtr<sl::float4>(sl::MEM::GPU);
         msg->is_bigendian = __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__;
         msg->is_dense = true;
         msg->height = bgraGpu.getHeight();
@@ -53,7 +58,7 @@ namespace mrover {
         Point* pcGpuPtr = pcGpu.data().get();
         dim3 threadsPerBlock{BLOCK_SIZE};
         dim3 numBlocks{static_cast<uint>(std::ceil(static_cast<float>(size) / BLOCK_SIZE))};
-        fillPointCloudMessageKernel<<<numBlocks, threadsPerBlock>>>(xyzGpuPtr, bgraGpuPtr, pcGpuPtr, size);
+        fillPointCloudMessageKernel<<<numBlocks, threadsPerBlock>>>(xyzGpuPtr, bgraGpuPtr, normalsGpuPtr, pcGpuPtr, size);
         checkCudaError(cudaPeekAtLastError());
         checkCudaError(cudaMemcpy(msg->data.data(), pcGpuPtr, size * sizeof(Point), cudaMemcpyDeviceToHost));
     }

src/perception/zed_wrapper/zed_wrapper.cpp

@@ -68,6 +68,7 @@ namespace mrover {
 
             mImageResolution = sl::Resolution(imageWidth, imageHeight);
             mPointResolution = sl::Resolution(imageWidth, imageHeight);
+            mNormalsResolution = sl::Resolution(imageWidth, imageHeight);
 
             NODELET_INFO("Resolution: %s image: %zux%zu points: %zux%zu",
                          grabResolutionString.c_str(), mImageResolution.width, mImageResolution.height, mPointResolution.width, mPointResolution.height);
@@ -139,7 +140,7 @@ namespace mrover {
                     mIsSwapReady = false;
                     mPcThreadProfiler.measureEvent("Wait");
 
-                    fillPointCloudMessageFromGpu(mPcMeasures.leftPoints, mPcMeasures.leftImage, mPointCloudGpu, pointCloudMsg);
+                    fillPointCloudMessageFromGpu(mPcMeasures.leftPoints, mPcMeasures.leftImage, mPcMeasures.leftNormals, mPointCloudGpu, pointCloudMsg);
                     pointCloudMsg->header.seq = mPointCloudUpdateTick;
                     pointCloudMsg->header.stamp = mPcMeasures.time;
                     pointCloudMsg->header.frame_id = "zed2i_left_camera_frame";
@@ -227,6 +228,8 @@ namespace mrover {
                     throw std::runtime_error("ZED failed to retrieve left image");
                 if (mZed.retrieveMeasure(mGrabMeasures.leftPoints, sl::MEASURE::XYZ, sl::MEM::GPU, mPointResolution) != sl::ERROR_CODE::SUCCESS)
                     throw std::runtime_error("ZED failed to retrieve point cloud");
+                if (mZed.retrieveMeasure(mGrabMeasures.leftNormals, sl::MEASURE::NORMALS, sl::MEM::GPU, mNormalsResolution) != sl::ERROR_CODE::SUCCESS)
+                    throw std::runtime_error("ZED failed to retrieve point cloud normals");
 
                 assert(mGrabMeasures.leftImage.timestamp == mGrabMeasures.leftPoints.timestamp);
 
@@ -315,6 +318,7 @@ namespace mrover {
         sl::Mat::swap(other.leftImage, leftImage);
         sl::Mat::swap(other.rightImage, rightImage);
         sl::Mat::swap(other.leftPoints, leftPoints);
+        sl::Mat::swap(other.leftNormals, leftNormals);
         std::swap(time, other.time);
         return *this;
     }

src/perception/zed_wrapper/zed_wrapper.hpp

@@ -12,6 +12,7 @@ namespace mrover {
             sl::Mat leftImage;
             sl::Mat rightImage;
             sl::Mat leftPoints;
+            sl::Mat leftNormals;
 
             Measures() = default;
 
@@ -31,7 +32,7 @@ namespace mrover {
 
         PointCloudGpu mPointCloudGpu;
 
-        sl::Resolution mImageResolution, mPointResolution;
+        sl::Resolution mImageResolution, mPointResolution, mNormalsResolution;
         sl::String mSvoPath;
         int mGrabTargetFps{};
         int mDepthConfidence{};
@@ -70,7 +71,7 @@ namespace mrover {
 
     auto slTime2Ros(sl::Timestamp t) -> ros::Time;
 
-    auto fillPointCloudMessageFromGpu(sl::Mat& xyzGpu, sl::Mat& bgraGpu, PointCloudGpu& pcGpu, sensor_msgs::PointCloud2Ptr const& msg) -> void;
+    void fillPointCloudMessageFromGpu(sl::Mat& xyzGpu, sl::Mat& bgraGpu, sl::Mat& normalsGpu, PointCloudGpu& pcGpu, sensor_msgs::PointCloud2Ptr const& msg);
 
     auto fillCameraInfoMessages(sl::CalibrationParameters& calibration, sl::Resolution const& resolution,
                                 sensor_msgs::CameraInfoPtr const& leftInfoMsg, sensor_msgs::CameraInfoPtr const& rightInfoMsg) -> void;

src/simulator/simulator.cpp

@@ -67,7 +67,7 @@ namespace mrover {
 #else
             glfwPollEvents();
 #endif
-            // glfwSetInputMode(mWindow.get(), GLFW_CURSOR, mInGui ? GLFW_CURSOR_NORMAL : GLFW_CURSOR_DISABLED);
+            glfwSetInputMode(mWindow.get(), GLFW_CURSOR, mInGui ? GLFW_CURSOR_NORMAL : GLFW_CURSOR_DISABLED);
             mLoopProfiler.measureEvent("GLFW Events");
 
             userControls(dt);

src/util/lie/se3.cpp

@@ -1,7 +1,7 @@
 #include "se3.hpp"
 
-SE3 SE3::fromTfTree(tf2_ros::Buffer const& buffer, std::string const& fromFrameId, std::string const& toFrameId) {
-    geometry_msgs::TransformStamped transform = buffer.lookupTransform(fromFrameId, toFrameId, ros::Time{});
+SE3 SE3::fromTfTree(tf2_ros::Buffer const& buffer, std::string const& toFrameId, std::string const& fromFrameId) {
+    geometry_msgs::TransformStamped transform = buffer.lookupTransform(toFrameId, fromFrameId, ros::Time{});
     return fromTf(transform.transform);
 }
 

starter_project/autonomy/msg/StarterProjectTag.msg

@@ -0,0 +1,4 @@
+int32 tagId
+float32 xTagCenterPixel
+float32 yTagCenterPixel
+float32 closenessMetric

starter_project/autonomy/src/perception.cpp

@@ -51,6 +51,8 @@ namespace mrover {
         // hint: you have mTagDictionary, mTagCorners, mTagIds, and mTagDetectorParams member variables already defined!
         // hint: write and use the "getCenterFromTagCorners" and "getClosenessMetricFromTagCorners" functions
 
+        cv::aruco::detectMarkers(image, mTagDictionary, mTagCorners, mTagIds, mTagDetectorParams);
+
         tags.clear(); // Clear old tags in output vector
 
         // TODO: implement me!

urdf/rover/rover.urdf.xacro

@@ -278,7 +278,7 @@
   </link>
 
   <joint name="chassis_link_to_long_range_camera_link" type="fixed">
-    <origin xyz="0.3 0 1"/>
+    <origin xyz="0.3 0 0"/>
     <parent link="chassis_link"/>
     <child link="long_range_camera_link"/>
   </joint>