Skip to content

Commit

Permalink
Feature/better path (#18)
Browse files Browse the repository at this point in the history 
* Commented out code where pointcloud edited, now expects pre-aggregated cloud. Changed to use sensor data QOS

* Reverted QOS change, barely helped and instead will fix with composable

* Added heartbeat timer pub

* Reduced KT horizon to 10, bugfix in task manager node name since recent updates
  • Loading branch information
@rien88
rien88 authored Nov 7, 2024
1 parent 589b606 commit d8dfa02
Show file tree
Hide file tree
Showing 3 changed files with 75 additions and 60 deletions.
43 changes: 29 additions & 14 deletions path_planning/src/path_planning.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@ Eigen::Vector3d cur_pos;
std::shared_ptr<rclcpp::Node> node;

//ROS publishers
rclcpp::Publisher<std_msgs::msg::Header>::SharedPtr heartbeat_pub;
rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr vis_pub;
rclcpp::Publisher<trajectory_msgs::msg::MultiDOFJointTrajectory>::SharedPtr traj_pub;
rclcpp::Publisher<trajectory_msgs::msg::MultiDOFJointTrajectory>::SharedPtr Bspline_pub;
Expand All @@ -34,13 +35,17 @@ rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr vel_pub;
rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr acc_pub;
rclcpp::Publisher<std_msgs::msg::Int64>::SharedPtr path_size_pub;

rclcpp::TimerBase::SharedPtr hb_timer;

ofstream outfile;

unique_ptr<KinodynamicAstar> kino_path_finder_;

std::string map_frame_;
std::string pose_topic_;

using namespace std::chrono_literals;

class planner {
public:
void init_start(void)
Expand Down Expand Up @@ -380,28 +385,34 @@ 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<pcl::PointXYZ> 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<pcl::PointXYZ> 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();
}

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)
{
Expand Down Expand Up @@ -466,8 +477,12 @@ int main(int argc, char **argv)
auto goal_sub = node->create_subscription<geometry_msgs::msg::PoseStamped>("/goal", 10000, goalCb);
auto time_index_sub = node->create_subscription<std_msgs::msg::Int64>("/demo_node/trajectory_time_index", 1000, timeindexCallBack);

rclcpp::QoS hb_qos(10);
hb_qos.liveliness();
heartbeat_pub = node->create_publisher<std_msgs::msg::Header>("/path_planner/heartbeat", hb_qos );
vis_pub = node->create_publisher<visualization_msgs::msg::Marker>("visualization_marker", 0 );
traj_pub = node->create_publisher<trajectory_msgs::msg::MultiDOFJointTrajectory>("waypoints",1);
hb_timer = node->create_wall_timer(500ms, heartbeatTimerCallback);

kino_pub = node->create_publisher<visualization_msgs::msg::Marker>("kino_marker", 1);
kino_path_pub = node->create_publisher<nav_msgs::msg::Path>("kino_path", 1);
Expand Down
2 changes: 1 addition & 1 deletion path_searching/include/path_searching/kinodynamic_astar.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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'
Expand Down
90 changes: 45 additions & 45 deletions path_searching/src/kinodynamic_astar.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,71 +30,71 @@ KinodynamicAstar::~KinodynamicAstar()

//Time accumulated KD-Tree realize
void KinodynamicAstar::setKdtree(const pcl::PointCloud<pcl::PointXYZ> 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<pcl::PointXYZ>::Ptr cloud_filtered;
cloud_filtered = cloud_accumulate.makeShared();;
cloud_filtered = cloud_input.makeShared();

// ROS_INFO("transfer success");
// // ROS_INFO("transfer success");

pcl::VoxelGrid<pcl::PointXYZ> sor;
sor.setInputCloud (cloud_filtered);
sor.setLeafSize (0.1f, 0.1f, 0.1f);
sor.filter (*cloud_filtered);
// pcl::VoxelGrid<pcl::PointXYZ> 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() <<endl;
// rclcpp::Time t2 = node_->get_clock()->now();
// outfile<< (t2-t1).seconds() <<endl;

// ROS_INFO("Pointcloud input to KDTREE used %f s, cloud size = %d",(t2-t1).toSec(),cloud_size);

cloud_input_num++;
// cloud_input_num++;

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);
// 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){
Expand Down Expand Up @@ -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<rclcpp::AsyncParametersClient>(node, "task_manager");
auto parameters_client = std::make_shared<rclcpp::AsyncParametersClient>(node, "/task_manager/task_manager");

std::vector<std::string> parameter_names = {"min_alt", "max_alt"};
auto get_parameters_future = parameters_client->get_parameters(parameter_names);
Expand Down

0 comments on commit d8dfa02

Please sign in to comment.