Program Listing for File drive_on_heading.hpp
↰ Return to documentation for file (include/nav2_behaviors/plugins/drive_on_heading.hpp
)
// Copyright (c) 2018 Intel Corporation
// Copyright (c) 2022 Joshua Wallace
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIORS__PLUGINS__DRIVE_ON_HEADING_HPP_
#define NAV2_BEHAVIORS__PLUGINS__DRIVE_ON_HEADING_HPP_
#include <chrono>
#include <memory>
#include <utility>
#include "nav2_behaviors/timed_behavior.hpp"
#include "nav2_msgs/action/drive_on_heading.hpp"
#include "nav2_msgs/action/back_up.hpp"
#include "nav2_util/node_utils.hpp"
#include "geometry_msgs/msg/twist_stamped.hpp"
namespace nav2_behaviors
{
template<typename ActionT = nav2_msgs::action::DriveOnHeading>
class DriveOnHeading : public TimedBehavior<ActionT>
{
using CostmapInfoType = nav2_core::CostmapInfoType;
public:
DriveOnHeading()
: TimedBehavior<ActionT>(),
feedback_(std::make_shared<typename ActionT::Feedback>()),
command_x_(0.0),
command_speed_(0.0),
simulate_ahead_time_(0.0)
{
}
~DriveOnHeading() = default;
ResultStatus onRun(const std::shared_ptr<const typename ActionT::Goal> command) override
{
if (command->target.y != 0.0 || command->target.z != 0.0) {
RCLCPP_INFO(
this->logger_,
"DrivingOnHeading in Y and Z not supported, will only move in X.");
return ResultStatus{Status::FAILED, ActionT::Result::INVALID_INPUT};
}
// Ensure that both the speed and direction have the same sign
if (!((command->target.x > 0.0) == (command->speed > 0.0)) ) {
RCLCPP_ERROR(this->logger_, "Speed and command sign did not match");
return ResultStatus{Status::FAILED, ActionT::Result::INVALID_INPUT};
}
command_x_ = command->target.x;
command_speed_ = command->speed;
command_time_allowance_ = command->time_allowance;
end_time_ = this->clock_->now() + command_time_allowance_;
if (!nav2_util::getCurrentPose(
initial_pose_, *this->tf_, this->local_frame_, this->robot_base_frame_,
this->transform_tolerance_))
{
RCLCPP_ERROR(this->logger_, "Initial robot pose is not available.");
return ResultStatus{Status::FAILED, ActionT::Result::TF_ERROR};
}
return ResultStatus{Status::SUCCEEDED, ActionT::Result::NONE};
}
ResultStatus onCycleUpdate() override
{
rclcpp::Duration time_remaining = end_time_ - this->clock_->now();
if (time_remaining.seconds() < 0.0 && command_time_allowance_.seconds() > 0.0) {
this->stopRobot();
RCLCPP_WARN(
this->logger_,
"Exceeded time allowance before reaching the DriveOnHeading goal - Exiting DriveOnHeading");
return ResultStatus{Status::FAILED, ActionT::Result::NONE};
}
geometry_msgs::msg::PoseStamped current_pose;
if (!nav2_util::getCurrentPose(
current_pose, *this->tf_, this->local_frame_, this->robot_base_frame_,
this->transform_tolerance_))
{
RCLCPP_ERROR(this->logger_, "Current robot pose is not available.");
return ResultStatus{Status::FAILED, ActionT::Result::TF_ERROR};
}
double diff_x = initial_pose_.pose.position.x - current_pose.pose.position.x;
double diff_y = initial_pose_.pose.position.y - current_pose.pose.position.y;
double distance = hypot(diff_x, diff_y);
feedback_->distance_traveled = distance;
this->action_server_->publish_feedback(feedback_);
if (distance >= std::fabs(command_x_)) {
this->stopRobot();
return ResultStatus{Status::SUCCEEDED, ActionT::Result::NONE};
}
auto cmd_vel = std::make_unique<geometry_msgs::msg::TwistStamped>();
cmd_vel->header.stamp = this->clock_->now();
cmd_vel->header.frame_id = this->robot_base_frame_;
cmd_vel->twist.linear.y = 0.0;
cmd_vel->twist.angular.z = 0.0;
cmd_vel->twist.linear.x = command_speed_;
geometry_msgs::msg::Pose2D pose2d;
pose2d.x = current_pose.pose.position.x;
pose2d.y = current_pose.pose.position.y;
pose2d.theta = tf2::getYaw(current_pose.pose.orientation);
if (!isCollisionFree(distance, cmd_vel->twist, pose2d)) {
this->stopRobot();
RCLCPP_WARN(this->logger_, "Collision Ahead - Exiting DriveOnHeading");
return ResultStatus{Status::FAILED, ActionT::Result::COLLISION_AHEAD};
}
this->vel_pub_->publish(std::move(cmd_vel));
return ResultStatus{Status::RUNNING, ActionT::Result::NONE};
}
CostmapInfoType getResourceInfo() override {return CostmapInfoType::LOCAL;}
protected:
bool isCollisionFree(
const double & distance,
const geometry_msgs::msg::Twist & cmd_vel,
geometry_msgs::msg::Pose2D & pose2d)
{
// Simulate ahead by simulate_ahead_time_ in this->cycle_frequency_ increments
int cycle_count = 0;
double sim_position_change;
const double diff_dist = abs(command_x_) - distance;
const int max_cycle_count = static_cast<int>(this->cycle_frequency_ * simulate_ahead_time_);
geometry_msgs::msg::Pose2D init_pose = pose2d;
bool fetch_data = true;
while (cycle_count < max_cycle_count) {
sim_position_change = cmd_vel.linear.x * (cycle_count / this->cycle_frequency_);
pose2d.x = init_pose.x + sim_position_change * cos(init_pose.theta);
pose2d.y = init_pose.y + sim_position_change * sin(init_pose.theta);
cycle_count++;
if (diff_dist - abs(sim_position_change) <= 0.) {
break;
}
if (!this->local_collision_checker_->isCollisionFree(pose2d, fetch_data)) {
return false;
}
fetch_data = false;
}
return true;
}
void onConfigure() override
{
auto node = this->node_.lock();
if (!node) {
throw std::runtime_error{"Failed to lock node"};
}
nav2_util::declare_parameter_if_not_declared(
node,
"simulate_ahead_time", rclcpp::ParameterValue(2.0));
node->get_parameter("simulate_ahead_time", simulate_ahead_time_);
}
typename ActionT::Feedback::SharedPtr feedback_;
geometry_msgs::msg::PoseStamped initial_pose_;
double command_x_;
double command_speed_;
rclcpp::Duration command_time_allowance_{0, 0};
rclcpp::Time end_time_;
double simulate_ahead_time_;
};
} // namespace nav2_behaviors
#endif // NAV2_BEHAVIORS__PLUGINS__DRIVE_ON_HEADING_HPP_