From 5362fce6fc8a5b6cd3213b2ef7b786b7d9926342 Mon Sep 17 00:00:00 2001 From: rien88 Date: Sat, 26 Oct 2024 15:08:10 -0400 Subject: [PATCH 1/4] Commented out code where pointcloud edited, now expects pre-aggregated cloud. Changed to use sensor data QOS --- path_planning/src/path_planning.cpp | 40 ++++++----- path_searching/src/kinodynamic_astar.cpp | 88 ++++++++++++------------ 2 files changed, 65 insertions(+), 63 deletions(-) diff --git a/path_planning/src/path_planning.cpp b/path_planning/src/path_planning.cpp index 265fa39..6c7245a 100644 --- a/path_planning/src/path_planning.cpp +++ b/path_planning/src/path_planning.cpp @@ -380,25 +380,25 @@ void cloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) pcl::fromROSMsg(*msg, (cloud_input)); // RCLCPP_INFO("FROM ROS TO PCLOUD SUCESS"); - + // No need to do this anymore, input cloud is already aggregated //only input point clouds in 20 meters to reduce computation - rclcpp::Time t1 = node->get_clock()->now(); - pcl::PointCloud cloud_cutoff; - // RCLCPP_INFO("FROM ROS TO PCLOUD Size is %d",int(cloud_input.size())); - for (size_t i = 0; i < cloud_input.points.size(); i = i+2) - { - if(fabs(cloud_input.points[i].x - cur_pos(0))>20 || fabs(cloud_input.points[i].y - cur_pos(1))>20 || fabs(cloud_input.points[i].z - cur_pos(2))>20) - { - continue; - }else{ - cloud_cutoff.push_back(cloud_input.points[i]); - } - } + // rclcpp::Time t1 = node->get_clock()->now(); + // pcl::PointCloud cloud_cutoff; + // // RCLCPP_INFO("FROM ROS TO PCLOUD Size is %d",int(cloud_input.size())); + // for (size_t i = 0; i < cloud_input.points.size(); i = i+2) + // { + // if(fabs(cloud_input.points[i].x - cur_pos(0))>20 || fabs(cloud_input.points[i].y - cur_pos(1))>20 || fabs(cloud_input.points[i].z - cur_pos(2))>20) + // { + // continue; + // }else{ + // cloud_cutoff.push_back(cloud_input.points[i]); + // } + // } rclcpp::Time t2 = node->get_clock()->now(); // RCLCPP_INFO("Pointcloud CUTOFF used %f s",(t2-t1).toSec()); - kino_path_finder_->setKdtree(cloud_cutoff); + kino_path_finder_->setKdtree(cloud_input); planner_ptr->replan(); } @@ -460,10 +460,12 @@ int main(int argc, char **argv) node->get_parameter("search/pose_topic", pose_topic_); node->get_parameter("search/cloud", cloud_topic); - auto odom_sub = node->create_subscription("/mavros/odometry/out", 1, odomCb); - auto pointcloud_sub = node->create_subscription(cloud_topic, 1, cloudCallback); - auto pose_sub = node->create_subscription(pose_topic_, 100, poseCb); - auto goal_sub = node->create_subscription("/goal", 10000, goalCb); + auto sensor_qos = rclcpp::SensorDataQoS(); + + auto odom_sub = node->create_subscription("/mavros/odometry/out", sensor_qos, odomCb); + auto pointcloud_sub = node->create_subscription(cloud_topic, sensor_qos, cloudCallback); + auto pose_sub = node->create_subscription(pose_topic_, sensor_qos, poseCb); + auto goal_sub = node->create_subscription("/goal", sensor_qos, goalCb); auto time_index_sub = node->create_subscription("/demo_node/trajectory_time_index", 1000, timeindexCallBack); vis_pub = node->create_publisher("visualization_marker", 0 ); @@ -474,7 +476,7 @@ int main(int argc, char **argv) Bspline_pub = node->create_publisher("smooth_waypoints",1); - pos_pub = node->create_publisher("/search_node/trajectory_position", 1); + pos_pub = node->create_publisher("/search_node/trajectory_position", sensor_qos); vel_pub = node->create_publisher("/search_node/trajectory_velocity", 1); acc_pub = node->create_publisher("/search_node/trajectory_accel", 1); path_size_pub = node->create_publisher("/search_node/trajectory_path_size", 1); diff --git a/path_searching/src/kinodynamic_astar.cpp b/path_searching/src/kinodynamic_astar.cpp index 756f789..be76288 100644 --- a/path_searching/src/kinodynamic_astar.cpp +++ b/path_searching/src/kinodynamic_astar.cpp @@ -30,71 +30,71 @@ KinodynamicAstar::~KinodynamicAstar() //Time accumulated KD-Tree realize void KinodynamicAstar::setKdtree(const pcl::PointCloud cloud_input){ + // No need to do this anymore, input cloud is already aggregated + // rclcpp::Time t1 = node_->get_clock()->now(); - rclcpp::Time t1 = node_->get_clock()->now(); - - if(cloud_input_num >= KT_HORIZON * KT_NUM) - { - cloud_input_num = 0; - tree_input_num = 0; - }else{ - tree_input_num = floor(cloud_input_num/KT_HORIZON); - } + // if(cloud_input_num >= KT_HORIZON * KT_NUM) + // { + // cloud_input_num = 0; + // tree_input_num = 0; + // }else{ + // tree_input_num = floor(cloud_input_num/KT_HORIZON); + // } - // ROS_INFO("CLOUD INPUT NUM = %d",cloud_input_num); - if( cloud_input_num%KT_HORIZON == 0 ) - { - cloud_accumulate2 = cloud_accumulate; - cloud_accumulate.clear(); - cloud_accumulate = (cloud_input); - }else{ - cloud_temp = cloud_accumulate + (cloud_input); - cloud_accumulate = cloud_temp; - } + // // ROS_INFO("CLOUD INPUT NUM = %d",cloud_input_num); + // if( cloud_input_num%KT_HORIZON == 0 ) + // { + // cloud_accumulate2 = cloud_accumulate; + // cloud_accumulate.clear(); + // cloud_accumulate = (cloud_input); + // }else{ + // cloud_temp = cloud_accumulate + (cloud_input); + // cloud_accumulate = cloud_temp; + // } - rclcpp::Time t4 = node_->get_clock()->now(); - // Create the filtering object + // rclcpp::Time t4 = node_->get_clock()->now(); + // // Create the filtering object pcl::PointCloud::Ptr cloud_filtered; - cloud_filtered = cloud_accumulate.makeShared();; + cloud_filtered = cloud_input.makeShared(); - // ROS_INFO("transfer success"); + // // ROS_INFO("transfer success"); - pcl::VoxelGrid sor; - sor.setInputCloud (cloud_filtered); - sor.setLeafSize (0.1f, 0.1f, 0.1f); - sor.filter (*cloud_filtered); + // pcl::VoxelGrid sor; + // sor.setInputCloud (cloud_filtered); + // sor.setLeafSize (0.1f, 0.1f, 0.1f); + // sor.filter (*cloud_filtered); - cloud_accumulate = *cloud_filtered; + // cloud_accumulate = *cloud_filtered; - rclcpp::Time t3 = node_->get_clock()->now(); - // ROS_INFO("Pointcloud filter used %f s",(t3-t4).toSec()); + // rclcpp::Time t3 = node_->get_clock()->now(); + // // ROS_INFO("Pointcloud filter used %f s",(t3-t4).toSec()); - int cloud_size = (*cloud_filtered).points.size(); + // int cloud_size = (*cloud_filtered).points.size(); // ROS_INFO("tree INPUT NUM = %d",tree_input_num); kdtreeLocalMap[tree_input_num].setInputCloud(cloud_filtered);//(*cloud_filtered).makeShared() - rclcpp::Time t2 = node_->get_clock()->now(); - outfile<< (t2-t1).seconds() <get_clock()->now(); + // outfile<< (t2-t1).seconds() <get_clock()->now();//.fromSec(last_timestamp_lidar); - kdtreepointcloud.header.frame_id = map_frame_; - kd_ptcloud_pub_filtered->publish(kdtreepointcloud); + // sensor_msgs::msg::PointCloud2 kdtreepointcloud,kdtreepointcloud2; + // pcl::toROSMsg((*cloud_filtered), kdtreepointcloud);// + // kdtreepointcloud.header.stamp = node_->get_clock()->now();//.fromSec(last_timestamp_lidar); + // kdtreepointcloud.header.frame_id = map_frame_; + // kd_ptcloud_pub_filtered->publish(kdtreepointcloud); - pcl::toROSMsg(cloud_accumulate2, kdtreepointcloud2);// - kdtreepointcloud2.header.stamp = node_->get_clock()->now();//.fromSec(last_timestamp_lidar); - kdtreepointcloud2.header.frame_id = map_frame_; - kd_ptcloud_pub_accumulated->publish(kdtreepointcloud2); + // pcl::toROSMsg(cloud_accumulate2, kdtreepointcloud2);// + // kdtreepointcloud2.header.stamp = node_->get_clock()->now();//.fromSec(last_timestamp_lidar); + // kdtreepointcloud2.header.frame_id = map_frame_; + // kd_ptcloud_pub_accumulated->publish(kdtreepointcloud2); - cloud_all = cloud_all + cloud_input; + // cloud_all = cloud_all + cloud_input; } bool KinodynamicAstar::isSafe(double x, double y,double z){ From 8c5658a91572dc6c2c8e2fba2e3aaf24bc6c6052 Mon Sep 17 00:00:00 2001 From: rien88 Date: Sat, 26 Oct 2024 18:41:45 -0400 Subject: [PATCH 2/4] Reverted QOS change, barely helped and instead will fix with composable --- path_planning/src/path_planning.cpp | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/path_planning/src/path_planning.cpp b/path_planning/src/path_planning.cpp index 6c7245a..9845b90 100644 --- a/path_planning/src/path_planning.cpp +++ b/path_planning/src/path_planning.cpp @@ -460,12 +460,10 @@ int main(int argc, char **argv) node->get_parameter("search/pose_topic", pose_topic_); node->get_parameter("search/cloud", cloud_topic); - auto sensor_qos = rclcpp::SensorDataQoS(); - - auto odom_sub = node->create_subscription("/mavros/odometry/out", sensor_qos, odomCb); - auto pointcloud_sub = node->create_subscription(cloud_topic, sensor_qos, cloudCallback); - auto pose_sub = node->create_subscription(pose_topic_, sensor_qos, poseCb); - auto goal_sub = node->create_subscription("/goal", sensor_qos, goalCb); + auto odom_sub = node->create_subscription("/mavros/odometry/out", 1, odomCb); + auto pointcloud_sub = node->create_subscription(cloud_topic, 1, cloudCallback); + auto pose_sub = node->create_subscription(pose_topic_, 100, poseCb); + auto goal_sub = node->create_subscription("/goal", 10000, goalCb); auto time_index_sub = node->create_subscription("/demo_node/trajectory_time_index", 1000, timeindexCallBack); vis_pub = node->create_publisher("visualization_marker", 0 ); @@ -476,7 +474,7 @@ int main(int argc, char **argv) Bspline_pub = node->create_publisher("smooth_waypoints",1); - pos_pub = node->create_publisher("/search_node/trajectory_position", sensor_qos); + pos_pub = node->create_publisher("/search_node/trajectory_position", 1); vel_pub = node->create_publisher("/search_node/trajectory_velocity", 1); acc_pub = node->create_publisher("/search_node/trajectory_accel", 1); path_size_pub = node->create_publisher("/search_node/trajectory_path_size", 1); From a92177b83890b779576cdbfedd6d92737eaeb73b Mon Sep 17 00:00:00 2001 From: rien88 Date: Sun, 3 Nov 2024 21:27:09 -0500 Subject: [PATCH 3/4] Added heartbeat timer pub --- path_planning/src/path_planning.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/path_planning/src/path_planning.cpp b/path_planning/src/path_planning.cpp index 9845b90..67404aa 100644 --- a/path_planning/src/path_planning.cpp +++ b/path_planning/src/path_planning.cpp @@ -23,6 +23,7 @@ Eigen::Vector3d cur_pos; std::shared_ptr node; //ROS publishers +rclcpp::Publisher::SharedPtr heartbeat_pub; rclcpp::Publisher::SharedPtr vis_pub; rclcpp::Publisher::SharedPtr traj_pub; rclcpp::Publisher::SharedPtr Bspline_pub; @@ -34,6 +35,8 @@ rclcpp::Publisher::SharedPtr vel_pub; rclcpp::Publisher::SharedPtr acc_pub; rclcpp::Publisher::SharedPtr path_size_pub; +rclcpp::TimerBase::SharedPtr hb_timer; + ofstream outfile; unique_ptr kino_path_finder_; @@ -41,6 +44,8 @@ unique_ptr kino_path_finder_; std::string map_frame_; std::string pose_topic_; +using namespace std::chrono_literals; + class planner { public: void init_start(void) @@ -402,6 +407,12 @@ void cloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) planner_ptr->replan(); } +void heartbeatTimerCallback() { + std_msgs::msg::Header hb; + hb.frame_id = "path_planner"; // Not used + heartbeat_pub->publish(hb); +} + nav_msgs::msg::Odometry uav_odometry; void odomCb(const nav_msgs::msg::Odometry::SharedPtr msg) { @@ -466,8 +477,12 @@ int main(int argc, char **argv) auto goal_sub = node->create_subscription("/goal", 10000, goalCb); auto time_index_sub = node->create_subscription("/demo_node/trajectory_time_index", 1000, timeindexCallBack); + rclcpp::QoS hb_qos(10); + hb_qos.liveliness(); + heartbeat_pub = node->create_publisher("/path_planner/heartbeat", hb_qos ); vis_pub = node->create_publisher("visualization_marker", 0 ); traj_pub = node->create_publisher("waypoints",1); + hb_timer = node->create_wall_timer(500ms, heartbeatTimerCallback); kino_pub = node->create_publisher("kino_marker", 1); kino_path_pub = node->create_publisher("kino_path", 1); From bf3ce51a51325f35b8809b1f29092cd5f9a26d5e Mon Sep 17 00:00:00 2001 From: rien88 Date: Sun, 3 Nov 2024 21:27:47 -0500 Subject: [PATCH 4/4] Reduced KT horizon to 10, bugfix in task manager node name since recent updates --- path_searching/include/path_searching/kinodynamic_astar.h | 2 +- path_searching/src/kinodynamic_astar.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/path_searching/include/path_searching/kinodynamic_astar.h b/path_searching/include/path_searching/kinodynamic_astar.h index 89f752c..36e4971 100644 --- a/path_searching/include/path_searching/kinodynamic_astar.h +++ b/path_searching/include/path_searching/kinodynamic_astar.h @@ -28,7 +28,7 @@ using namespace std; //TIme accumulated KD-Tree parameters -#define KT_HORIZON 50 +#define KT_HORIZON 10 #define KT_NUM 1 #define IN_CLOSE_SET 'a' diff --git a/path_searching/src/kinodynamic_astar.cpp b/path_searching/src/kinodynamic_astar.cpp index be76288..cf66a47 100644 --- a/path_searching/src/kinodynamic_astar.cpp +++ b/path_searching/src/kinodynamic_astar.cpp @@ -144,7 +144,7 @@ int KinodynamicAstar::search(Eigen::Vector3d start_pt, Eigen::Vector3d start_v, Eigen::Vector3d end_pt, Eigen::Vector3d end_v, bool init, bool dynamic, double time_start) { auto node = rclcpp::Node::make_shared("parameter_client_node"); - auto parameters_client = std::make_shared(node, "task_manager"); + auto parameters_client = std::make_shared(node, "/task_manager/task_manager"); std::vector parameter_names = {"min_alt", "max_alt"}; auto get_parameters_future = parameters_client->get_parameters(parameter_names);