use pose message type and update readme

README.md

@@ -14,8 +14,8 @@ TODO (make diagrams!)
 
 | Dependency | Required Version/Ref | Description |
 | - | - | - |
-| [sick_scan_xd](https://github.com/sickag/sick_scan_xd) | The latest release | Lidar interfacing - provices point clouds and IMU measurements |
-| [direct_lidar_inertial_odometry](https://github.com/vectr-ucla/direct_lidar_inertial_odometry) | The `feature/ros2` branch | Localization - uses SLAM to produce a location and orientation estimate |
+| [sick_scan_xd](https://github.com/sickag/sick_scan_xd) | The latest release | Lidar interfacing - provices point clouds and IMU measurements. |
+| [direct_lidar_inertial_odometry](https://github.com/vectr-ucla/direct_lidar_inertial_odometry) | The `feature/ros2` branch, unless a ROS1->ROS2 bridge is utilized. | Localization - uses SLAM to produce a location and orientation estimate. |
 
 ## Initialization / Updating
 The project includes submodules for all relevant ROS dependencies (and some vestigial dependencies). These are not required to build the project, but may be utilized as an easy way to pull all required external packages if they need to built and installed. Use the `--recurse-submodules` option when cloning fresh and `git submodule update --init --recursive` for an already cloned project or when changing/updating branches.
@@ -27,7 +27,7 @@ Since the project targets ROS2, it can be built using Colcon (which uses CMake).
 |-|-|-|-|
 | [PCL](https://github.com/PointCloudLibrary/pcl) *(Point Cloud Library)* | Must be manually installed. On linux, the easiest method is to use `sudo apt-get install libpcl-dev`. For windows, the PCL GitHub repository provides an installer for each release. | Versions 1.12.0 and up have been verified to work. | Make sure to add the relevant directories to PATH when installing on windows (see the PCL docs installation guide) so that CMake can properly find the libraries and headers. |
 | [Eigen](https://gitlab.com/libeigen/eigen) | Eigen is a dependenciy of PCL and is thus almost gaurenteed to always be present by default. In any case, external versions can used as well, and can be sourced from the link provided. | Not super relevant. Any version compatible with PCL will suffice. | n/a |
-| [pcl_ros](https://github.com/ros-perception/perception_pcl) | Must be manually built and sourced as a ROS package. | The correct branch for whatever ROS distro is in use. | `pcl_ros` is a subproject in the `perception_pcl` repository. It must be manually built and sourced. |
+| [pcl_ros](https://github.com/ros-perception/perception_pcl) | Must be manually built and sourced as a ROS package. | The correct branch for whatever ROS distro is in use. | `pcl_ros` is a subproject in the `perception_pcl` repository. It must be manually built and sourced before building this project. |
 
 For the most part, the project is configured to use the optimal build variables by default, however, the provided options and some common CMake options are listed here:
 - `sickperception_USE_WPILIB_TESTING` -- Configures whether or not WPILib will be included (built) and if dependant logging/debug functionality (NetworkTables) will be enabled.
@@ -43,7 +43,20 @@ For the most part, the project is configured to use the optimal build variables
 
   *Note that 'sickperception_\*' refers to a project-wide configuration, while 'LDRP_\*' (stands for LiDaR Perception) refers to a code-specific \[internal\] configuration.*
 
-TODO: build commands + examples here
+---
+**To build, use Colcon (when inside the top-level project directory):**
+```
+colcon build (--event-handlers console_direct+)
+```
+**Then source the package:**
+```
+source ./install/setup.bash
+```
+**Finally, the node can be run using:**
+```
+ros2 run sick_perception ldrp_node
+```
+*More detailed configurations and launch methods to come...*
 
 ## Usage
 

src/ldrp/perception.cpp

@@ -59,6 +59,7 @@
 #include "./filtering.hpp"
 
 #include <limits>
+#include <memory>
 #include <type_traits>
 
 #include <pcl/common/transforms.h>
@@ -94,11 +95,11 @@ namespace util {
 PerceptionNode::PerceptionNode() : rclcpp::Node("perception_node") {
 	RCLCPP_INFO(this->get_logger(), "Perception Node Initialization!");
 	// setup subs/pubs
-	this->scan_sub = this->create_subscription<sensor_msgs::msg::PointCloud2>("input_scan", 1,
+	this->scan_sub = this->create_subscription<sensor_msgs::msg::PointCloud2>("/cloud_all_fields_fullframe", 1,
 		std::bind(&PerceptionNode::scan_cb, this, std::placeholders::_1));
-	this->trfm_sub = this->create_subscription<geometry_msgs::msg::TransformStamped>("input_trfm", 1,
-		std::bind(&PerceptionNode::trfm_cb, this, std::placeholders::_1));
-	this->grid_pub = this->create_publisher<nav_msgs::msg::OccupancyGrid>("obstacle_grid", 1);
+	this->pose_sub = this->create_subscription<geometry_msgs::msg::PoseStamped>("/undecided_yet", 1,
+		std::bind(&PerceptionNode::pose_cb, this, std::placeholders::_1));
+	this->grid_pub = this->create_publisher<nav_msgs::msg::OccupancyGrid>("/obstacle_grid", 1);
 
 	//get parameters
 	util::declare_param(this, "map_resolution_cm", this->_config.map_resolution_cm, this->_config.map_resolution_cm);
@@ -174,10 +175,10 @@ void PerceptionNode::scan_cb(const sensor_msgs::msg::PointCloud2::ConstSharedPtr
 }
 
 
-void PerceptionNode::trfm_cb(const geometry_msgs::msg::TransformStamped::ConstSharedPtr& trfm) {
+void PerceptionNode::pose_cb(const geometry_msgs::msg::PoseStamped::ConstSharedPtr& pose) {
 	RCLCPP_INFO(this->get_logger(), "Transform callback called!");
 
-	const int64_t trfm_target_ts = util::constructTimestampMicros<uint32_t>(trfm->header.stamp.sec, trfm->header.stamp.nanosec);
+	const int64_t trfm_target_ts = util::constructTimestampMicros<uint32_t>(pose->header.stamp.sec, pose->header.stamp.nanosec);
 	const typename decltype(this->scan_sampler)::ElemT* sample = this->scan_sampler.sampleTimestamped(trfm_target_ts);
 
 	// if scan doesn't exist
@@ -207,15 +208,15 @@ void PerceptionNode::trfm_cb(const geometry_msgs::msg::TransformStamped::ConstSh
 		);
 
 		const Eigen::Quaternionf quat{
-			static_cast<float>(trfm->transform.rotation.w),
-			static_cast<float>(trfm->transform.rotation.x),
-			static_cast<float>(trfm->transform.rotation.y),
-			static_cast<float>(trfm->transform.rotation.z)
+			static_cast<float>(pose->pose.orientation.w),
+			static_cast<float>(pose->pose.orientation.x),
+			static_cast<float>(pose->pose.orientation.y),
+			static_cast<float>(pose->pose.orientation.z)
 		};
 		const Eigen::Translation3f pos{
-			static_cast<float>(trfm->transform.translation.x),
-			static_cast<float>(trfm->transform.translation.y),
-			static_cast<float>(trfm->transform.translation.z)
+			static_cast<float>(pose->pose.position.x),
+			static_cast<float>(pose->pose.position.y),
+			static_cast<float>(pose->pose.position.z)
 		};
 
 		pcl::transformPointCloud(

src/ldrp/perception.hpp

@@ -3,15 +3,13 @@
 #include "./grid.hpp"
 #include "./timestamp_sampler.hpp"
 
-#include <memory>
-
 #include <Eigen/Core>
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
-#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
 #include <nav_msgs/msg/occupancy_grid.hpp>
 
 
@@ -23,15 +21,15 @@ class PerceptionNode : public rclcpp::Node {
 
 protected:
 	void scan_cb(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& scan);
-	void trfm_cb(const geometry_msgs::msg::TransformStamped::ConstSharedPtr& trfm);
+	void pose_cb(const geometry_msgs::msg::PoseStamped::ConstSharedPtr& pose);
 
 	void process(const pcl::PointCloud<pcl::PointXYZ>& cloud, const Eigen::Vector3f& origin);
 	void process_and_export(const pcl::PointCloud<pcl::PointXYZ>& cloud, const Eigen::Vector3f& origin);
 
 
 protected:
 	rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr scan_sub;
-	rclcpp::Subscription<geometry_msgs::msg::TransformStamped>::SharedPtr trfm_sub;
+	rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub;
 	rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr grid_pub;
 
 	// last pointcloud and transform?