Skip to content

adding goal tolerance, laser reference adjustment, loggers #46

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

Open
padhupradheep wants to merge 2 commits into
base: main
Choose a base branch
from
Open

adding goal tolerance, laser reference adjustment, loggers #46

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
66edbe3
adding goal tolerance, laser reference adjustment, loggers
padhupradheep Nov 12, 2024
fb2e886
handling scanners both for the corners and in the middle
padhupradheep Nov 14, 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
5 changes: 4 additions & 1 deletion launch/dock_pose.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,4 +8,7 @@
- 0.99999992074214661
- 0.0
- 0.0
- 0.00039814030247637615
- 0.00039814030247637615
laser_ref: 0.32
xy_goal_tolerance: 0.005
corner_scanners: True #depends on the laser scanner range set for the laserscanners provided in the robots' configs.
27 changes: 21 additions & 6 deletions src/neo_docking.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -62,10 +62,14 @@ class NeoDocking
this->declare_parameter<std::vector<double>>("pose", {-1, 0, 0});
this->declare_parameter<std::vector<double>>("orientation", {0, 0, 0.707, 0.707});
this->declare_parameter<double>("laser_ref", 0.32);
this->declare_parameter<double>("xy_goal_tolerance", 0.005);
this->declare_parameter<bool>("corner_scanners", true);

this->get_parameter("pose", pose_array_);
this->get_parameter("orientation", orientation_array_);
this->get_parameter("laser_ref", laser_ref_);
this->get_parameter("xy_goal_tolerance", goal_tol_);
this->get_parameter("corner_scanners", corner_scanners_);

tf_static_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);

Expand All @@ -74,7 +78,7 @@ class NeoDocking

robot_namespace.erase(0,1);

if (robot_namespace != "/")
if (robot_namespace != "")
{
RCLCPP_INFO(this->get_logger(), "automatically configuring namespace support");
docking_station_ = robot_namespace + "/" + docking_station_;
Expand Down Expand Up @@ -154,23 +158,26 @@ class NeoDocking

// determine the distance of the robot from docking station
double distance = euclidean_distance(robot_pose, checkTransform);
geometry_msgs::msg::Twist twist_vel;

// additionaly layer check if docking has completed
if (distance <= 0.015) {
if (distance <= goal_tol_) {
RCLCPP_INFO(client_node_->get_logger(), "Docking finished");
twist_vel.linear.x = 0.000; // Setting 0 velocity
vel_pub->publish(twist_vel);
RCLCPP_INFO(client_node_->get_logger(), "Distance to the stored pose %f", distance);
on_process_ = false;
nav_task_finished_ = false;
return;
}

auto robot_docking_pose = checkTransform;
geometry_msgs::msg::Twist twist_vel;

/** setting conditions for the robot to dock
* distance between the robot and docking station will vary
* depending on the localization. Therefore, using laser-
* reference to halt the robot **/
if (distance > 0.015 && laser_ref_ < store_laser_ref_) {
if (distance > goal_tol_ && (laser_ref_ - 0.01) < store_laser_ref_) {
try {
robot_pose = buffer_->lookupTransform("map", base_link_, tf2::TimePointZero);
} catch (const std::exception & ex) {
Expand All @@ -182,8 +189,9 @@ class NeoDocking
vel_pub->publish(twist_vel);
} else {
RCLCPP_INFO(client_node_->get_logger(), "Docking finished");
twist_vel.linear.x = 0.0; // Setting 0 velocity
twist_vel.linear.x = 0.000; // Setting 0 velocity
vel_pub->publish(twist_vel);
RCLCPP_INFO(client_node_->get_logger(), "Distance to the stored pose %f", distance);
on_process_ = false;
nav_task_finished_ = false;
}
Expand All @@ -205,7 +213,12 @@ class NeoDocking
void scan_callback(const sensor_msgs::msg::LaserScan::SharedPtr sensor_data)
{
auto data = sensor_data;
store_laser_ref_ = data->ranges[static_cast<int>(data->ranges.size()) / 2];
if (corner_scanners_) {
store_laser_ref_ = data->ranges[static_cast<int>(data->ranges.size()) - 1];
} else {
store_laser_ref_ = data->ranges[static_cast<int>(data->ranges.size()) / 2];
}

}

void result_callback(const WaypointFollowerGoalHandle::WrappedResult & result)
Expand Down Expand Up @@ -529,6 +542,8 @@ class NeoDocking

double laser_ref_ = 0.0;
double store_laser_ref_ = 0.0;
double goal_tol_ = 0.0;
bool corner_scanners_ = true;

std::string robot_namespace;

Expand Down