hongshaorou/YOPO
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

no visualization of trajecotries if no subscription

Browse Source
This commit is contained in:
TJU_Lu 2024-12-18 11:49:34 +08:00
parent e3f7af7f01
commit d21a2f818b
3 changed files with 20 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
flightlib/include/flightlib/ros_nodes/flight_pilot.hpp
View File

@ -36,9 +36,9 @@ class FlightPilot {
void poseCallback(const nav_msgs::Odometry::ConstPtr& msg);
// unity
bool setUnity(const bool render);
bool setUnity(void);
bool connectUnity(void);
bool disconnectUnity();
void disconnectUnity(void);
bool loadParams(const YAML::Node& cfg);
bool configCamera(const YAML::Node& cfg);
void computeDepthImage(const cv::Mat& left_frame, const cv::Mat& right_frame, cv::Mat* const depth);

19
flightlib/src/ros_nodes/flight_pilot.cpp
View File

@ -32,10 +32,12 @@ FlightPilot::FlightPilot(const ros::NodeHandle& nh, const ros::NodeHandle& pnh)
timestamp = ros::Time::now();
// connect unity and setup unity
setUnity(unity_render_);
setUnity();
connectUnity();
if (!unity_ready_)
if (!unity_ready_){
ROS_ERROR("[FlightRos] Connection Faild! Start the Flightmare Unity First!");
exit(1);
}
spawnTreesAndSavePointCloud();
timer_main_loop_ = nh_.createTimer(ros::Rate(main_loop_freq_), &FlightPilot::mainLoopCallback, this);
@ -137,8 +139,7 @@ void FlightPilot::mainLoopCallback(const ros::TimerEvent& event) {
}
}
bool FlightPilot::setUnity(const bool render) {
unity_render_ = render;
bool FlightPilot::setUnity() {
if (unity_render_ && unity_bridge_ptr_ == nullptr) {
// create unity bridge
unity_bridge_ptr_ = UnityBridge::getInstance();
@ -171,9 +172,9 @@ bool FlightPilot::connectUnity() {
return unity_ready_;
}
bool FlightPilot::disconnectUnity() {
void FlightPilot::disconnectUnity() {
if (unity_render_ && unity_bridge_ptr_ != nullptr)
;
return;
unity_bridge_ptr_->disconnectUnity();
unity_ready_ = false;
}
@ -243,8 +244,7 @@ bool FlightPilot::configCamera(const YAML::Node& cfg) {
std::vector<Scalar> r_BC_vec = cfg["rgb_camera_left"]["r_BC"].as<std::vector<Scalar>>();
Vector<3> t_BC(t_BC_vec.data());
Matrix<3, 3> r_BC = (AngleAxis(r_BC_vec[0] * M_PI / 180.0, Vector<3>::UnitX()) * AngleAxis(r_BC_vec[1] * M_PI / 180.0, Vector<3>::UnitY()) *
AngleAxis(r_BC_vec[2] * M_PI / 180.0, Vector<3>::UnitZ()))
.toRotationMatrix(); // the rotation order has been verified
AngleAxis(r_BC_vec[2] * M_PI / 180.0, Vector<3>::UnitZ())).toRotationMatrix(); // the rotation order has been verified
// Convert the horizontal FOV (usually used) to vertical FOV (flightmare).
Scalar rgb_fov_deg_ = cfg["rgb_camera_left"]["fov"].as<Scalar>();
double hor_fov_radians = (M_PI * (rgb_fov_deg_ / 180.0));
@ -272,8 +272,7 @@ bool FlightPilot::configCamera(const YAML::Node& cfg) {
Vector<3> t_BC_r(t_BC_vec_r.data());
Matrix<3, 3> r_BC_r = (AngleAxis(r_BC_vec[0] * M_PI / 180.0, Vector<3>::UnitX()) * AngleAxis(r_BC_vec[1] * M_PI / 180.0, Vector<3>::UnitY()) *
AngleAxis(r_BC_vec[2] * M_PI / 180.0, Vector<3>::UnitZ()))
.toRotationMatrix();
AngleAxis(r_BC_vec[2] * M_PI / 180.0, Vector<3>::UnitZ())).toRotationMatrix();
rgb_camera_right->setFOV(flightmare_fov);
rgb_camera_right->setWidth(cfg["rgb_camera_left"]["width"].as<int>());

15
flightlib/src/ros_nodes/yopo_planner_node.cpp
View File

@ -95,18 +95,21 @@ void yopo_cb(const std_msgs::Float32MultiArray::ConstPtr msg) {
// std::cout<<"yopo开始预测到当前时间: "<<delta_t<<std::endl;
sample_t = 0.0; // = delta_t
pcl::PointCloud<pcl::PointXYZI>::Ptr best_traj_cld(new pcl::PointCloud<pcl::PointXYZI>);
traj_to_pcl(traj_opt_bridge, best_traj_cld);
pcl_conversions::toPCL(ros::Time::now(), best_traj_cld->header.stamp); // for test
best_traj_cld->header.frame_id = "world";
best_traj_visual_pub.publish(best_traj_cld);
if (best_traj_visual_pub.getNumSubscribers() > 0){
pcl::PointCloud<pcl::PointXYZI>::Ptr best_traj_cld(new pcl::PointCloud<pcl::PointXYZI>);
traj_to_pcl(traj_opt_bridge, best_traj_cld);
pcl_conversions::toPCL(ros::Time::now(), best_traj_cld->header.stamp);
best_traj_cld->header.frame_id = "world";
best_traj_visual_pub.publish(best_traj_cld);
}
yopo_init = true;
}
void trajs_vis_cb(const std_msgs::Float32MultiArray::ConstPtr msg) {
if (!odom_init)
return;
if ((trajs_visual_pub.getNumSubscribers() == 0) && (lattice_trajs_visual_pub.getNumSubscribers() == 0))
return;
// ---------------- visualization of all trajs --------------------
std::vector<std::vector<double>> endstates_b;
endstates_b.resize(msg->layout.dim[0].size);