Skip to content

Tharp/random walk bug fixes #139

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

Merged
tharp789 merged 2 commits into from
Nov 8, 2024
Merged

Tharp/random walk bug fixes #139

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
c4fbc39
fixing goal point generation, sampling previsouly occured randomly an…
tharp789 Nov 8, 2024
7d20090
cleaning up frame references
tharp789 Nov 8, 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
3 changes: 1 addition & 2 deletions robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/config/random_walk_config.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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"
Expand All @@ -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
Expand Down
4 changes: 2 additions & 2 deletions robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/include/random_walk_logic.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@ typedef std::vector<std::tuple<float, float, float, float>> 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;
Expand All @@ -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;

Expand Down
11 changes: 7 additions & 4 deletions robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/include/random_walk_node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,8 @@
#include <std_srvs/srv/trigger.hpp>
#include <string>
#include <tf2_msgs/msg/tf_message.hpp>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>
#include <tuple>
#include <vector>
#include <visualization_msgs/msg/marker.hpp>
Expand Down Expand Up @@ -68,17 +70,18 @@ class RandomWalkNode : public rclcpp::Node {
// Other functions
std::optional<init_params> 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<tf2_ros::Buffer> tf_buffer;
std::shared_ptr<tf2_ros::TransformListener> tf_listener;

// ROS subscribers
rclcpp::Subscription<visualization_msgs::msg::Marker>::SharedPtr sub_map;
rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr sub_robot_tf;
// rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr sub_robot_tf;

// ROS publishers
rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr pub_global_plan;
Expand Down
29 changes: 16 additions & 13 deletions robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/src/random_walk_logic.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -21,7 +21,7 @@ std::optional<Path> 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::seconds>(
std::chrono::steady_clock::now() - start_time)
.count() < timeout_duration) {
Expand Down Expand Up @@ -103,14 +103,17 @@ std::tuple<float, float, float> 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<float>(rand()) / static_cast<float>(RAND_MAX) * 2.0f *
this->max_start_to_goal_dist_m_) -
this->max_start_to_goal_dist_m_;
float rand_y = (static_cast<float>(rand()) / static_cast<float>(RAND_MAX) * 2.0f *
this->max_start_to_goal_dist_m_) -
this->max_start_to_goal_dist_m_;
float rand_z = static_cast<float>(rand()) / static_cast<float>(RAND_MAX) * this->max_z_m_;
std::random_device rd;
std::mt19937 gen(rd());
std::uniform_real_distribution<float> distribution_xy(-this->max_start_to_goal_dist_m_,
this->max_start_to_goal_dist_m_);
std::uniform_real_distribution<float> 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<float, float, float> rand_point(rand_x, rand_y, rand_z);
std::tuple<float, float, float> start_point_wo_yaw(
std::get<0>(start_point), std::get<1>(start_point), std::get<2>(start_point));
Expand All @@ -119,15 +122,15 @@ std::tuple<float, float, float> 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;
}
}
Expand Down
99 changes: 22 additions & 77 deletions robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/src/random_walk_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,11 +6,6 @@
std::optional<init_params> RandomWalkNode::readParameters() {
// Read in parameters based off the default yaml file
init_params params;
this->declare_parameter<std::string>("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<init_params>{};
}
this->declare_parameter<std::string>("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");
Expand Down Expand Up @@ -66,9 +61,9 @@ std::optional<init_params> RandomWalkNode::readParameters() {
RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: checking_point_cnt");
return std::optional<init_params>{};
}
this->declare_parameter<float>("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<float>("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<init_params>{};
}
this->declare_parameter<float>("collision_padding_m");
Expand Down Expand Up @@ -99,9 +94,10 @@ RandomWalkNode::RandomWalkNode() : Node("random_walk_node") {

this->sub_map = this->create_subscription<visualization_msgs::msg::Marker>(
sub_map_topic_, 10, std::bind(&RandomWalkNode::mapCallback, this, std::placeholders::_1));
this->sub_robot_tf = this->create_subscription<tf2_msgs::msg::TFMessage>(
sub_robot_tf_topic_, 10,
std::bind(&RandomWalkNode::tfCallback, this, std::placeholders::_1));

//TF buffer and listener
tf_buffer = std::make_shared<tf2_ros::Buffer>(this->get_clock());
tf_listener = std::make_shared<tf2_ros::TransformListener>(*tf_buffer);

this->pub_global_plan = this->create_publisher<nav_msgs::msg::Path>(pub_global_plan_topic_, 10);
this->pub_goal_point =
Expand Down Expand Up @@ -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<float, float, float>(msg->scale.x, msg->scale.y, msg->scale.z);
Expand Down Expand Up @@ -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;
Expand All @@ -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++) {
Expand All @@ -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[]) {
Expand Down