Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Feature/better path #18

Merged
merged 4 commits into from
Nov 7, 2024
Merged

Feature/better path #18

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
5362fce
Commented out code where pointcloud edited, now expects pre-aggregate…
rien88 Oct 26, 2024
8c5658a
Reverted QOS change, barely helped and instead will fix with composable
rien88 Oct 26, 2024
a92177b
Added heartbeat timer pub
rien88 Nov 4, 2024
bf3ce51
Reduced KT horizon to 10, bugfix in task manager node name since rece…
rien88 Nov 4, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
43 changes: 29 additions & 14 deletions path_planning/src/path_planning.cpp
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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