From 0714e5cc057d5f7a943d75f8c922b371d744c016 Mon Sep 17 00:00:00 2001 From: Sam Richter Date: Sat, 27 Apr 2024 01:00:15 -0500 Subject: [PATCH] add nt bridge for grid --- extra/nt-bridge/src/main.cpp | 27 +++++++++++++++++- src/ldrp/perception.cpp | 55 +++++++++++++++++++++--------------- 2 files changed, 58 insertions(+), 24 deletions(-) diff --git a/extra/nt-bridge/src/main.cpp b/extra/nt-bridge/src/main.cpp index 34a7379..9456384 100644 --- a/extra/nt-bridge/src/main.cpp +++ b/extra/nt-bridge/src/main.cpp @@ -51,12 +51,14 @@ class NTBridgeNode : public rclcpp::Node { this->nt_scan_sub = inst.GetRawTopic("uesim/scan").Subscribe( "PointXYZ_[]", {} ); this->nt_pose_sub = inst.GetFloatArrayTopic("uesim/pose").Subscribe( {} ); + this->nt_grid_pub = inst.GetRawTopic("nt-bridge/grid").Publish( "Grid_U8" ); inst.AddListener(this->nt_scan_sub, nt::EventFlags::kValueAll, std::bind(&NTBridgeNode::nt_scan_cb, this, std::placeholders::_1)); inst.AddListener(this->nt_pose_sub, nt::EventFlags::kValueAll, std::bind(&NTBridgeNode::nt_pose_cb, this, std::placeholders::_1)); this->scan_pub = this->create_publisher("/uesim/scan", 1); this->pose_pub = this->create_publisher("/uesim/pose", 1); + this->grid_sub = this->create_subscription("/ldrp/obstacle_grid", 1, std::bind(&NTBridgeNode::ros_grid_cb, this, std::placeholders::_1)); } ~NTBridgeNode() = default; @@ -129,16 +131,39 @@ class NTBridgeNode : public rclcpp::Node { } } } + void ros_grid_cb(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr& grid) { + const size_t len = static_cast( grid->info.width * grid->info.height ) + 20; + uint8_t* _data = new uint8_t[len]; + + reinterpret_cast(_data)[0] = static_cast( grid->info.width ); // x cells + reinterpret_cast(_data)[1] = static_cast( grid->info.height ); // y cells + reinterpret_cast(_data)[2] = static_cast( grid->info.origin.position.x ); // x origin + reinterpret_cast(_data)[3] = static_cast( grid->info.origin.position.y ); // y origin + reinterpret_cast(_data)[4] = static_cast( grid->info.resolution ); + memcpy(_data + 20, grid->data.data(), grid->data.size()); + + this->nt_grid_pub.Set( + std::span{ + _data, + _data + len + }, + (static_cast(grid->header.stamp.sec) * 1000000L) + (static_cast(grid->header.stamp.nanosec) / 1000L) + ); + + delete[] _data; + RCLCPP_INFO(this->get_logger(), "Published Grid!"); + } protected: nt::RawSubscriber nt_scan_sub; nt::FloatArraySubscriber nt_pose_sub; + nt::RawPublisher nt_grid_pub; rclcpp::Publisher::SharedPtr scan_pub; rclcpp::Publisher::SharedPtr pose_pub; + rclcpp::Subscription::SharedPtr grid_sub; - // rclcpp::Subscription grid_sub; }; diff --git a/src/ldrp/perception.cpp b/src/ldrp/perception.cpp index 017190a..f527cfc 100644 --- a/src/ldrp/perception.cpp +++ b/src/ldrp/perception.cpp @@ -7,24 +7,33 @@ * See the License for the specific language governing permissions and * * limitations under the License. * * * -* ##**# * -* #%%#%%%@ * -* %%%#%%@@@@ * -* %%*%%%@@@ * -* +*%%@@@ * -* *%@@@%%%*=+%%% * -* +##++*++++*%@*++#%%% * -* +++#++*+++==*%%@#+++#%% * -* +++*#*++++++++*%%@@%*+*#*+* * -* *+*#%%@*+*+++*#%%%%@@@%++++*## * -* +++==*%#####*##%%%%%#***++*#%%% * -* #%%%###*+****=*%%%%%%%#%##****###%%%%%%%% * -* -----========+*#%%#%%%%%%@@@@%%%%%%%%%%%%%% * -* ---===++++++++**+=--==++++++++*##*++***+++**#%@@@ * -* --=+*##%%##%%###%%%*-----===++*#%###*#%%%%%%%%%%@@ * -* ++#%%%%%%%%%%%%%@@@#=--===++*#%%%@@@#*#%@@@@@@@@ * -* *#%@@%%%%%@@@@@@@#=--=-::.....-+#%%@@*#%@@@@@@@ * -* %@@@@@@@@@@@@@@*--=-............::-+@@@@%%@@@@ * +* ;xxxxxxx: * +* ;$$$$$$$$$ ...::.. * +* $$$$$$$$$$x .:::::::::::.. * +* x$$$$$$$$$$$$$$::::::::::::::::. * +* :$$$$$&X; .xX:::::::::::::.::... * +* .$$Xx++$$$$+ :::. :;: .::::::. .... : * +* :$$$$$$$$$ ;: ;xXXXXXXXx .::. .::::. .:. * +* :$$$$$$$$: ; ;xXXXXXXXXXXXXx: ..:::::: .::. * +* ;$$$$$$$$ :: :;XXXXXXXXXXXXXXXXXX+ .::::. .::: * +* X$$$$$X : +XXXXXXXXXXXXXXXXXXXXXXXX; .:: .::::. * +* .$$$$ :xXXXXXXXXXXXXXXXXXXXXXXXXXXX. .:::::. * +* X$$X XXXXXXXXXXXXXXXXXXXXXXXXXXXXx: .::::. * +* $$$:.XXXXXXXXXXXXXXXXXXXXXXXXXXX ;; ..:. * +* $$& :XXXXXXXXXXXXXXXXXXXXXXXX; +XX; X$$; * +* $$$::XXXXXXXXXXXXXXXXXXXXXX: :XXXXX; X$$; * +* X$$X XXXXXXXXXXXXXXXXXXX; .+XXXXXXX; $$$ * +* $$$$ ;XXXXXXXXXXXXXXX+ +XXXXXXXXx+ X$$$+ * +* x$$$$$X ;XXXXXXXXXXX+ :xXXXXXXXX+ .;$$$$$$ * +* +$$$$$$$$ ;XXXXXXx;;+XXXXXXXXX+ : +$$$$$$$$ * +* +$$$$$$$$: xXXXXXXXXXXXXXX+ ; X$$$$$$$$ * +* :$$$$$$$$$. +XXXXXXXXX: ;: x$$$$$$$$$ * +* ;x$$$$XX$$$$+ .;+X+ :;: :$$$$$xX$$$X * +* ;;;;;;;;;;X$$$$$$$+ :X$$$$$$&. * +* ;;;;;;;:;;;;;x$$$$$$$$$$$$$$$$x. * +* :;;;;;;;;;;;;. :$$$$$$$$$$X * +* .;;;;;;;;:;; +$$$$$$$$$ * +* .;;;;;;. X$$$$$$$: * * * *******************************************************************************/ /** FOR REFERENCE @@ -178,7 +187,7 @@ void PerceptionNode::scan_cb(const sensor_msgs::msg::PointCloud2::ConstSharedPtr this->sampler_mutex.unlock(); RCLCPP_INFO(this->get_logger(), - "\n\tScan sample timestamp (ms): %ld" + "\n\tScan sample timestamp (us): %ld" "\n\tTotal scan samples: %ld", ts, current_nsamples @@ -201,7 +210,7 @@ void PerceptionNode::pose_cb(const geometry_msgs::msg::PoseStamped::ConstSharedP if(!sample) { RCLCPP_INFO(this->get_logger(), "Invalid sample recieved!" - "\n\tTransform timestamp (ms): %ld", + "\n\tTransform timestamp (us): %ld", trfm_target_ts ); // this->sampler_mutex.unlock(); @@ -236,9 +245,9 @@ void PerceptionNode::pose_cb(const geometry_msgs::msg::PoseStamped::ConstSharedP this->sampler_mutex.unlock(); RCLCPP_INFO(this->get_logger(), - "\n\tTransform timestamp (ms): %ld" - "\n\tSampled timestamp (ms): %ld" - "\n\tTimestamp difference (ms): %ld" + "\n\tTransform timestamp (us): %ld" + "\n\tSampled timestamp (us): %ld" + "\n\tTimestamp difference (us): %ld" "\n\tTotal scan samples: %ld", trfm_target_ts, sample->first,