diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/config/random_walk_config.yaml b/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/config/random_walk_config.yaml index 5386249cd..74ae6fa09 100644 --- a/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/config/random_walk_config.yaml +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/config/random_walk_config.yaml @@ -1,6 +1,5 @@ /**: ros__parameters: - world_frame_id: "map" robot_frame_id: "base_link" pub_global_plan_topic: "~/global_plan" pub_goal_point_viz_topic: "~/goal_point_viz" @@ -15,7 +14,7 @@ # Constants max_start_to_goal_dist_m: 10.0 # how max distance the goal can be from the current robot position checking_point_cnt: 1000 # how many points to collision check between the start and goal points - max_z_m: 0.1 # max height that the goal can deviate from the current height + max_z_change_m: 0.1 # max height that the goal can deviate from the current height collision_padding_m: 0.1 # how much space should the path have to any obstacles path_end_threshold_m: 0.1 # how close to the goal will the path be considered complete max_z_angle_change_rad: 1.5708 # how much the goal can deviate from the current orientation diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/include/random_walk_logic.hpp b/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/include/random_walk_logic.hpp index 9b69babe5..0a95c1d50 100644 --- a/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/include/random_walk_logic.hpp +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/include/random_walk_logic.hpp @@ -16,7 +16,7 @@ typedef std::vector> Path; // x, y, z, y struct init_params { float max_start_to_goal_dist_m; int checking_point_cnt; - float max_z_m; + float max_z_change_m; float collision_padding_m; float path_end_threshold_m; float max_z_angle_change_rad; @@ -40,7 +40,7 @@ class RandomWalkPlanner { // Numerical constants float max_start_to_goal_dist_m_; int checking_point_cnt; - float max_z_m_; + float max_z_change_m_; float collision_padding_m; float max_z_angle_change_rad; diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/include/random_walk_node.hpp b/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/include/random_walk_node.hpp index 896e5c213..1a7eb45d1 100644 --- a/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/include/random_walk_node.hpp +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/include/random_walk_node.hpp @@ -15,6 +15,8 @@ #include #include #include +#include +#include #include #include #include @@ -68,17 +70,18 @@ class RandomWalkNode : public rclcpp::Node { // Other functions std::optional readParameters(); - visualization_msgs::msg::Marker createGoalPointMarker(); - visualization_msgs::msg::Marker createTrajectoryLineMarker(); - public: // explicit RandomWalkNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); RandomWalkNode(); ~RandomWalkNode() = default; + // TF buffer + std::shared_ptr tf_buffer; + std::shared_ptr tf_listener; + // ROS subscribers rclcpp::Subscription::SharedPtr sub_map; - rclcpp::Subscription::SharedPtr sub_robot_tf; + // rclcpp::Subscription::SharedPtr sub_robot_tf; // ROS publishers rclcpp::Publisher::SharedPtr pub_global_plan; diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/src/random_walk_logic.cpp b/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/src/random_walk_logic.cpp index 812430c58..2ce0503ed 100644 --- a/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/src/random_walk_logic.cpp +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/src/random_walk_logic.cpp @@ -6,7 +6,7 @@ RandomWalkPlanner::RandomWalkPlanner(init_params params) { this->max_start_to_goal_dist_m_ = params.max_start_to_goal_dist_m; this->checking_point_cnt = params.checking_point_cnt; - this->max_z_m_ = params.max_z_m; + this->max_z_change_m_ = params.max_z_change_m; this->voxel_size_m = params.voxel_size_m; this->collision_padding_m = params.collision_padding_m; this->path_end_threshold_m = params.path_end_threshold_m; @@ -21,7 +21,7 @@ std::optional RandomWalkPlanner::generate_straight_rand_path( bool is_goal_point_valid = false; // get start ti const std::chrono::steady_clock::time_point start_time = std::chrono::steady_clock::now(); - RCLCPP_INFO(rclcpp::get_logger("random_walk_planner"), "Starting Path Search..."); + // RCLCPP_INFO(rclcpp::get_logger("random_walk_planner"), "Starting Path Search..."); while (!is_goal_point_valid && std::chrono::duration_cast( std::chrono::steady_clock::now() - start_time) .count() < timeout_duration) { @@ -103,14 +103,17 @@ std::tuple RandomWalkPlanner::generate_goal_point( const clock_t start_time = clock(); while ((clock() - start_time) / CLOCKS_PER_SEC < time_out_duration) { - // TODO: make this random generation limited by some input to generate less points - float rand_x = (static_cast(rand()) / static_cast(RAND_MAX) * 2.0f * - this->max_start_to_goal_dist_m_) - - this->max_start_to_goal_dist_m_; - float rand_y = (static_cast(rand()) / static_cast(RAND_MAX) * 2.0f * - this->max_start_to_goal_dist_m_) - - this->max_start_to_goal_dist_m_; - float rand_z = static_cast(rand()) / static_cast(RAND_MAX) * this->max_z_m_; + std::random_device rd; + std::mt19937 gen(rd()); + std::uniform_real_distribution distribution_xy(-this->max_start_to_goal_dist_m_, + this->max_start_to_goal_dist_m_); + std::uniform_real_distribution distribution_z(-this->max_z_change_m_, this->max_z_change_m_); + float delta_x = distribution_xy(gen); + float delta_y = distribution_xy(gen); + float delta_z = distribution_z(gen); + float rand_x = std::get<0>(start_point) + delta_x; + float rand_y = std::get<1>(start_point) + delta_y; + float rand_z = std::get<2>(start_point) + delta_z; std::tuple rand_point(rand_x, rand_y, rand_z); std::tuple start_point_wo_yaw( std::get<0>(start_point), std::get<1>(start_point), std::get<2>(start_point)); @@ -119,15 +122,15 @@ std::tuple RandomWalkPlanner::generate_goal_point( std::get<0>(rand_point) - std::get<0>(start_point_wo_yaw)); float angle_diff = std::abs(std::get<3>(start_point) - new_angle); // if the z value of the point is low enough - if (rand_z < max_z_m_) { + if (rand_z < max_z_change_m_) { // if the angle change is low enough if (angle_diff < this->max_z_angle_change_rad) { // if the point is close enough to the start point if (dist < this->max_start_to_goal_dist_m_) { // if the point doesnt collide with any of the voxels if (!(check_if_collided(rand_point))) { - RCLCPP_INFO(rclcpp::get_logger("random_walk_planner"), - "Angle Difference: %f", rad2deg(angle_diff)); + // RCLCPP_INFO(rclcpp::get_logger("random_walk_planner"), + // "Angle Difference: %f", rad2deg(angle_diff)); return rand_point; } } diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/src/random_walk_node.cpp b/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/src/random_walk_node.cpp index 25a98f5d7..d75081e52 100644 --- a/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/src/random_walk_node.cpp +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/src/random_walk_node.cpp @@ -6,11 +6,6 @@ std::optional RandomWalkNode::readParameters() { // Read in parameters based off the default yaml file init_params params; - this->declare_parameter("world_frame_id"); - if (!this->get_parameter("world_frame_id", this->world_frame_id_)) { - RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: world_frame_id"); - return std::optional{}; - } this->declare_parameter("robot_frame_id"); if (!this->get_parameter("robot_frame_id", this->robot_frame_id_)) { RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: robot_frame_id"); @@ -66,9 +61,9 @@ std::optional RandomWalkNode::readParameters() { RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: checking_point_cnt"); return std::optional{}; } - this->declare_parameter("max_z_m"); - if (!this->get_parameter("max_z_m", params.max_z_m)) { - RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: max_z_m"); + this->declare_parameter("max_z_change_m"); + if (!this->get_parameter("max_z_change_m", params.max_z_change_m)) { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: max_z_change_m"); return std::optional{}; } this->declare_parameter("collision_padding_m"); @@ -99,9 +94,10 @@ RandomWalkNode::RandomWalkNode() : Node("random_walk_node") { this->sub_map = this->create_subscription( sub_map_topic_, 10, std::bind(&RandomWalkNode::mapCallback, this, std::placeholders::_1)); - this->sub_robot_tf = this->create_subscription( - sub_robot_tf_topic_, 10, - std::bind(&RandomWalkNode::tfCallback, this, std::placeholders::_1)); + + //TF buffer and listener + tf_buffer = std::make_shared(this->get_clock()); + tf_listener = std::make_shared(*tf_buffer); this->pub_global_plan = this->create_publisher(pub_global_plan_topic_, 10); this->pub_goal_point = @@ -139,6 +135,7 @@ void RandomWalkNode::mapCallback(const visualization_msgs::msg::Marker::SharedPt // updating the local voxel points and generating a path only if the path is not executing if (!this->received_first_map) { this->received_first_map = true; + this->world_frame_id_ = msg->header.frame_id; RCLCPP_INFO(this->get_logger(), "Received first map"); this->params.voxel_size_m = std::tuple(msg->scale.x, msg->scale.y, msg->scale.z); @@ -234,15 +231,6 @@ void RandomWalkNode::generate_plan() { point_msg.header.stamp = this->now(); generated_single_path.poses.push_back(point_msg); } - // this->pub_global_path->publish(this->generated_path); - if (this->publish_visualizations) { - this->pub_goal_point->publish(createGoalPointMarker()); - this->pub_trajectory_lines->publish(createTrajectoryLineMarker()); - } - // RCLCPP_INFO(this->get_logger(), "Published path and goal point at %f, %f, %f", - // this->generated_path.poses.back().pose.position.x, - // this->generated_path.poses.back().pose.position.y, - // this->generated_path.poses.back().pose.position.z); geometry_msgs::msg::PoseStamped last_goal_loc = generated_single_path.poses.back(); this->current_goal_location.translation.x = last_goal_loc.pose.position.x; this->current_goal_location.translation.y = last_goal_loc.pose.position.y; @@ -268,6 +256,20 @@ void RandomWalkNode::publish_plan() { RCLCPP_INFO(this->get_logger(), "Published full path"); } void RandomWalkNode::timerCallback() { + // get current TF to world + try { + geometry_msgs::msg::TransformStamped transform_stamped = + this->tf_buffer->lookupTransform(this->world_frame_id_, this->robot_frame_id_, + rclcpp::Time(0)); + this->current_location = transform_stamped.transform; + if (!this->received_first_robot_tf) { + this->received_first_robot_tf = true; + RCLCPP_INFO(this->get_logger(), "Received first robot_tf"); + } + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR(this->get_logger(), "Could not get robot tf: %s", ex.what()); + } + if (this->enable_random_walk && !this->is_path_executing) { if (this->received_first_map && this->received_first_robot_tf) { for (int i = 0; i < this->num_paths_to_generate_; i++) { @@ -292,63 +294,6 @@ void RandomWalkNode::timerCallback() { RCLCPP_INFO(this->get_logger(), "Reached goal point"); } } - // if (this->publish_visualizations && this->generated_path.poses.size() > 0) { - // visualization_msgs::msg::Marker goal_point_msg = createGoalPointMarker(); - // this->pub_goal_point->publish(goal_point_msg); - // visualization_msgs::msg::Marker trajectory_line_msg = createTrajectoryLineMarker(); - // this->pub_trajectory_lines->publish(trajectory_line_msg); - // RCLCPP_INFO(this->get_logger(), "Published goal point and trajectory line"); - // } -} - -visualization_msgs::msg::Marker RandomWalkNode::createTrajectoryLineMarker() { - visualization_msgs::msg::Marker trajectory_line_msg; - trajectory_line_msg.header.frame_id = this->world_frame_id_; - trajectory_line_msg.header.stamp = this->now(); - trajectory_line_msg.ns = "trajectory_line"; - trajectory_line_msg.id = 0; - trajectory_line_msg.type = visualization_msgs::msg::Marker::LINE_STRIP; - trajectory_line_msg.action = visualization_msgs::msg::Marker::ADD; - trajectory_line_msg.pose.orientation.w = 1.0; - trajectory_line_msg.scale.x = 0.1; - trajectory_line_msg.color.a = 1.0; - trajectory_line_msg.color.r = 1.0; - trajectory_line_msg.color.g = 0.0; - trajectory_line_msg.color.b = 0.0; - - for (auto path : this->generated_paths) { - for (auto point : path.poses) { - geometry_msgs::msg::Point p; - p.x = point.pose.position.x; - p.y = point.pose.position.y; - p.z = point.pose.position.z; - trajectory_line_msg.points.push_back(p); - } - } - return trajectory_line_msg; -} - -visualization_msgs::msg::Marker RandomWalkNode::createGoalPointMarker() { - visualization_msgs::msg::Marker goal_point_msg; - goal_point_msg.header.frame_id = this->world_frame_id_; - goal_point_msg.header.stamp = this->now(); - goal_point_msg.ns = "goal_point"; - goal_point_msg.id = 0; - goal_point_msg.type = visualization_msgs::msg::Marker::SPHERE; - goal_point_msg.action = visualization_msgs::msg::Marker::ADD; - geometry_msgs::msg::Point goal_point = geometry_msgs::msg::Point(); - goal_point.x = this->current_goal_location.translation.x; - goal_point.y = this->current_goal_location.translation.y; - goal_point.z = this->current_goal_location.translation.z; - goal_point_msg.pose.position = goal_point; - goal_point_msg.scale.x = 0.1; - goal_point_msg.scale.y = 0.1; - goal_point_msg.scale.z = 0.1; - goal_point_msg.color.a = 1.0; - goal_point_msg.color.r = 0.0; - goal_point_msg.color.g = 1.0; - goal_point_msg.color.b = 0.0; - return goal_point_msg; } int main(int argc, char *argv[]) {