Skip to content

Commit

Permalink
add nt bridge for grid
Browse files Browse the repository at this point in the history
  • Loading branch information
@S1ink
S1ink committed Apr 27, 2024
1 parent ff5481e commit 0714e5c
Show file tree
Hide file tree
Showing 2 changed files with 58 additions and 24 deletions.
27 changes: 26 additions & 1 deletion extra/nt-bridge/src/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<sensor_msgs::msg::PointCloud2>("/uesim/scan", 1);
this->pose_pub = this->create_publisher<geometry_msgs::msg::PoseStamped>("/uesim/pose", 1);
this->grid_sub = this->create_subscription<nav_msgs::msg::OccupancyGrid>("/ldrp/obstacle_grid", 1, std::bind(&NTBridgeNode::ros_grid_cb, this, std::placeholders::_1));
}
~NTBridgeNode() = default;

Expand Down Expand Up @@ -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<size_t>( grid->info.width * grid->info.height ) + 20;
uint8_t* _data = new uint8_t[len];

reinterpret_cast<int32_t*>(_data)[0] = static_cast<int32_t>( grid->info.width ); // x cells
reinterpret_cast<int32_t*>(_data)[1] = static_cast<int32_t>( grid->info.height ); // y cells
reinterpret_cast<float*>(_data)[2] = static_cast<float>( grid->info.origin.position.x ); // x origin
reinterpret_cast<float*>(_data)[3] = static_cast<float>( grid->info.origin.position.y ); // y origin
reinterpret_cast<float*>(_data)[4] = static_cast<float>( grid->info.resolution );
memcpy(_data + 20, grid->data.data(), grid->data.size());

this->nt_grid_pub.Set(
std::span<const uint8_t>{
_data,
_data + len
},
(static_cast<int64_t>(grid->header.stamp.sec) * 1000000L) + (static_cast<int64_t>(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<sensor_msgs::msg::PointCloud2>::SharedPtr scan_pub;
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pose_pub;
rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr grid_sub;

// rclcpp::Subscription<nav_msgs::msg::OccupancyGrid> grid_sub;

};

Expand Down
55 changes: 32 additions & 23 deletions src/ldrp/perception.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand All @@ -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();
Expand Down Expand Up @@ -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,
Expand Down

0 comments on commit 0714e5c

Please sign in to comment.