From aecfad0fb11a7c1f51fb77bfe8bafe906cc9a5c2 Mon Sep 17 00:00:00 2001 From: Steve Date: Wed, 15 May 2019 12:02:00 -0700 Subject: [PATCH 1/5] enabling closed-loop control in spinning --- .../nav2_motion_primitives/back_up.hpp | 2 +- .../include/nav2_motion_primitives/spin.hpp | 11 +-- nav2_motion_primitives/src/back_up.cpp | 12 +-- nav2_motion_primitives/src/spin.cpp | 73 +++---------------- 4 files changed, 20 insertions(+), 78 deletions(-) diff --git a/nav2_motion_primitives/include/nav2_motion_primitives/back_up.hpp b/nav2_motion_primitives/include/nav2_motion_primitives/back_up.hpp index caa964364c..5f9a84c9db 100644 --- a/nav2_motion_primitives/include/nav2_motion_primitives/back_up.hpp +++ b/nav2_motion_primitives/include/nav2_motion_primitives/back_up.hpp @@ -40,7 +40,7 @@ class BackUp : public MotionPrimitiveget_logger(), "Backing up in Y and Z not supported, " "will only move in X."); } - command_x_ = command->x; + command_x_2_ = command->x * command->x; if (!robot_->getOdometry(initial_pose_)) { RCLCPP_ERROR(node_->get_logger(), "initial robot odom pose is not available."); return nav2_tasks::TaskStatus::FAILED; @@ -55,15 +55,11 @@ nav2_tasks::TaskStatus BackUp::onRun(const nav2_tasks::BackUpCommand::SharedPtr nav2_tasks::TaskStatus BackUp::onCycleUpdate(nav2_tasks::BackUpResult & result) { TaskStatus status = controlledBackup(); - - // For now sending an empty task result nav2_tasks::BackUpResult empty_result; result = empty_result; - return status; } - nav2_tasks::TaskStatus BackUp::controlledBackup() { auto current_odom_pose = std::shared_ptr(); @@ -79,15 +75,15 @@ nav2_tasks::TaskStatus BackUp::controlledBackup() double diff_x = initial_pose_->pose.pose.position.x - current_odom_pose->pose.pose.position.x; double diff_y = initial_pose_->pose.pose.position.y - current_odom_pose->pose.pose.position.y; - double distance = sqrt(diff_x * diff_x + diff_y * diff_y); + double distance_2 = diff_x * diff_x + diff_y * diff_y; - if (distance >= abs(command_x_)) { + if (distance_2 >= abs(command_x_2_)) { cmd_vel.linear.x = 0; robot_->sendVelocity(cmd_vel); return TaskStatus::SUCCEEDED; } // TODO(mhpanah): cmd_vel value should be passed as a parameter - command_x_ < 0 ? cmd_vel.linear.x = -0.025 : cmd_vel.linear.x = 0.025; + command_x_2_ < 0 ? cmd_vel.linear.x = -0.025 : cmd_vel.linear.x = 0.025; robot_->sendVelocity(cmd_vel); return TaskStatus::RUNNING; diff --git a/nav2_motion_primitives/src/spin.cpp b/nav2_motion_primitives/src/spin.cpp index ffc2c1cb63..9ce1571c48 100644 --- a/nav2_motion_primitives/src/spin.cpp +++ b/nav2_motion_primitives/src/spin.cpp @@ -49,65 +49,21 @@ Spin::~Spin() nav2_tasks::TaskStatus Spin::onRun(const nav2_tasks::SpinCommand::SharedPtr command) { - double yaw, pitch, roll; - tf2::getEulerYPR(command->quaternion, yaw, pitch, roll); - - if (roll != 0.0 || pitch != 0.0) { - RCLCPP_INFO(node_->get_logger(), "Spinning on Y and X not supported, " - "will only spin in Z."); - } - - RCLCPP_INFO(node_->get_logger(), "Currently only supported spinning by a fixed amount"); - - start_time_ = std::chrono::system_clock::now(); - + double pitch, roll; + tf2::getEulerYPR(command->quaternion, start_yaw_, pitch, roll); return nav2_tasks::TaskStatus::SUCCEEDED; } nav2_tasks::TaskStatus Spin::onCycleUpdate(nav2_tasks::SpinResult & result) { - // Currently only an open-loop controller is implemented - // TODO(orduno) #423 Create a base class for open-loop controlled motion_primitives - // controlledSpin() has not been fully tested - TaskStatus status = timedSpin(); - - // For now sending an empty task result + TaskStatus status = rotateSome(); nav2_tasks::SpinResult empty_result; result = empty_result; - return status; } -nav2_tasks::TaskStatus Spin::timedSpin() +nav2_tasks::TaskStatus Spin::rotateSome() { - // Output control command - geometry_msgs::msg::Twist cmd_vel; - - // TODO(orduno) #423 fixed speed - cmd_vel.linear.x = 0.0; - cmd_vel.linear.y = 0.0; - cmd_vel.angular.z = 0.5; - robot_->sendVelocity(cmd_vel); - - // TODO(orduno) #423 fixed time - auto current_time = std::chrono::system_clock::now(); - if (current_time - start_time_ >= 6s) { // almost 180 degrees - // Stop the robot - cmd_vel.angular.z = 0.0; - robot_->sendVelocity(cmd_vel); - - return TaskStatus::SUCCEEDED; - } - - return TaskStatus::RUNNING; -} - -nav2_tasks::TaskStatus Spin::controlledSpin() -{ - // TODO(orduno) #423 Test and tune controller - // check it doesn't abruptly start and stop - // or cause massive wheel slippage when accelerating - // Get current robot orientation auto current_pose = std::make_shared(); if (!robot_->getCurrentPose(current_pose)) { @@ -115,20 +71,15 @@ nav2_tasks::TaskStatus Spin::controlledSpin() return TaskStatus::FAILED; } - double current_yaw = tf2::getYaw(current_pose->pose.pose.orientation); + // Get distance left + const double current_yaw = tf2::getYaw(current_pose->pose.pose.orientation); + const double current_angle_dist = current_yaw - start_yaw_; + const double dist_left = M_PI - current_angle_dist; - double current_angle = current_yaw - start_yaw_; + // TODO(stevemacenski) #553 check for collisions - double dist_left = M_PI - current_angle; - - // TODO(orduno) #379 forward simulation to check if future position is feasible - - // compute the velocity that will let us stop by the time we reach the goal - // v_f^2 == v_i^2 + 2 * a * d - // solving for v_i if v_f = 0 + // Compute the velocity, clip, and send double vel = sqrt(2 * rotational_acc_lim_ * dist_left); - - // limit velocity vel = std::min(std::max(vel, min_rotational_vel_), max_rotational_vel_); geometry_msgs::msg::Twist cmd_vel; @@ -138,8 +89,8 @@ nav2_tasks::TaskStatus Spin::controlledSpin() robot_->sendVelocity(cmd_vel); - // check if we are done - if (dist_left >= (0.0 - goal_tolerance_angle_)) { + // Termination conditions + if (fabs(dist_left) < goal_tolerance_angle_) { return TaskStatus::SUCCEEDED; } From 395f4488a0b66468183f869ff1d0317808808e50 Mon Sep 17 00:00:00 2001 From: Steve Date: Wed, 15 May 2019 16:30:20 -0700 Subject: [PATCH 2/5] backup back to square root rep. of distances --- .../include/nav2_motion_primitives/back_up.hpp | 2 +- nav2_motion_primitives/src/back_up.cpp | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/nav2_motion_primitives/include/nav2_motion_primitives/back_up.hpp b/nav2_motion_primitives/include/nav2_motion_primitives/back_up.hpp index 5f9a84c9db..caa964364c 100644 --- a/nav2_motion_primitives/include/nav2_motion_primitives/back_up.hpp +++ b/nav2_motion_primitives/include/nav2_motion_primitives/back_up.hpp @@ -40,7 +40,7 @@ class BackUp : public MotionPrimitiveget_logger(), "Backing up in Y and Z not supported, " "will only move in X."); } - command_x_2_ = command->x * command->x; + command_x_ = command->x; if (!robot_->getOdometry(initial_pose_)) { RCLCPP_ERROR(node_->get_logger(), "initial robot odom pose is not available."); return nav2_tasks::TaskStatus::FAILED; @@ -75,15 +75,15 @@ nav2_tasks::TaskStatus BackUp::controlledBackup() double diff_x = initial_pose_->pose.pose.position.x - current_odom_pose->pose.pose.position.x; double diff_y = initial_pose_->pose.pose.position.y - current_odom_pose->pose.pose.position.y; - double distance_2 = diff_x * diff_x + diff_y * diff_y; + double distance_ = sqrt(diff_x * diff_x + diff_y * diff_y); - if (distance_2 >= abs(command_x_2_)) { + if (distance_ >= abs(command_x_)) { cmd_vel.linear.x = 0; robot_->sendVelocity(cmd_vel); return TaskStatus::SUCCEEDED; } // TODO(mhpanah): cmd_vel value should be passed as a parameter - command_x_2_ < 0 ? cmd_vel.linear.x = -0.025 : cmd_vel.linear.x = 0.025; + command_x_ < 0 ? cmd_vel.linear.x = -0.025 : cmd_vel.linear.x = 0.025; robot_->sendVelocity(cmd_vel); return TaskStatus::RUNNING; From d7c9dc7ac8414855e37dc9d8cd6c169c85e4bc66 Mon Sep 17 00:00:00 2001 From: Steve Date: Fri, 17 May 2019 15:21:01 -0700 Subject: [PATCH 3/5] adding handling distances unnormalized 0->pi for all sets <=180 --- .../include/nav2_motion_primitives/spin.hpp | 4 +- nav2_motion_primitives/src/spin.cpp | 43 ++++++++++++++----- 2 files changed, 36 insertions(+), 11 deletions(-) diff --git a/nav2_motion_primitives/include/nav2_motion_primitives/spin.hpp b/nav2_motion_primitives/include/nav2_motion_primitives/spin.hpp index 12f67d4de3..26838958cf 100644 --- a/nav2_motion_primitives/include/nav2_motion_primitives/spin.hpp +++ b/nav2_motion_primitives/include/nav2_motion_primitives/spin.hpp @@ -43,7 +43,9 @@ class Spin : public MotionPrimitive(); } Spin::~Spin() @@ -50,8 +52,28 @@ Spin::~Spin() nav2_tasks::TaskStatus Spin::onRun(const nav2_tasks::SpinCommand::SharedPtr command) { - double pitch, roll; - tf2::getEulerYPR(command->quaternion, start_yaw_, pitch, roll); + double pitch_holder, roll_holder; + if (!robot_->getOdometry(initial_pose_)) { + RCLCPP_ERROR(node_->get_logger(), "initial robot odom pose is not available."); + return nav2_tasks::TaskStatus::FAILED; + } + + // getEulerYPR are normalized angles (-M_PI, M_PI] + tf2::getEulerYPR(command->quaternion, commanded_dist_, pitch_holder, roll_holder); + + if (roll_holder != 0.0 || pitch_holder != 0.0) { + RCLCPP_WARN(node_->get_logger(), "Spinning on Y and X not supported, " + "will only spin in Z."); + } + + // gets unsigned distance and turning direction TODO + if (commanded_dist_ < 0.0) { + commanded_dist_ += 2 * M_PI; + direction_ = -1.0; + } else { + direction_ = 1.0; + } + return nav2_tasks::TaskStatus::SUCCEEDED; } @@ -73,11 +95,10 @@ nav2_tasks::TaskStatus Spin::rotateSome() } // Get distance left - const double current_yaw = tf2::getYaw(current_pose->pose.pose.orientation); - const double current_angle_dist = current_yaw - start_yaw_; - const double dist_left = M_PI - current_angle_dist; + const double current_angle_dist = tf2::angleShortestPath(current_pose, initial_pose_); + const double dist_left = commanded_dist_ - fabs(current_angle_dist); - // TODO(stevemacenski) #553 check for collisions + // TODO(stevemacenski) #533 check for collisions // Compute the velocity, clip, and send double vel = sqrt(2 * rotational_acc_lim_ * dist_left); @@ -86,12 +107,14 @@ nav2_tasks::TaskStatus Spin::rotateSome() geometry_msgs::msg::Twist cmd_vel; cmd_vel.linear.x = 0.0; cmd_vel.linear.y = 0.0; - cmd_vel.angular.z = vel; + cmd_vel.angular.z = vel * direction_; robot_->sendVelocity(cmd_vel); // Termination conditions - if (fabs(dist_left) < goal_tolerance_angle_) { + if (dist_left < goal_tolerance_angle_) { + cmd_vel.angular.z = 0.0; + robot_->sendVelocity(cmd_vel); return TaskStatus::SUCCEEDED; } From 973eab13c338a32604641627b1f41ef870ccc39c Mon Sep 17 00:00:00 2001 From: Steve Date: Fri, 17 May 2019 15:29:17 -0700 Subject: [PATCH 4/5] remove initlaization of shared ptr and TODO log --- nav2_motion_primitives/src/spin.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/nav2_motion_primitives/src/spin.cpp b/nav2_motion_primitives/src/spin.cpp index 6b5b97c15e..895bbb7eaf 100644 --- a/nav2_motion_primitives/src/spin.cpp +++ b/nav2_motion_primitives/src/spin.cpp @@ -43,7 +43,6 @@ Spin::Spin(rclcpp::Node::SharedPtr & node) goal_tolerance_angle_ = 0.17; direction_ = 1.0; commanded_dist_ = 0.0; - initial_pose_ = std::make_shared(); } Spin::~Spin() @@ -66,7 +65,7 @@ nav2_tasks::TaskStatus Spin::onRun(const nav2_tasks::SpinCommand::SharedPtr comm "will only spin in Z."); } - // gets unsigned distance and turning direction TODO + // gets unsigned distance and turning direction if (commanded_dist_ < 0.0) { commanded_dist_ += 2 * M_PI; direction_ = -1.0; From 062cdf35081badf718bd66bdba225fa7590c768d Mon Sep 17 00:00:00 2001 From: Steve Date: Fri, 17 May 2019 15:31:10 -0700 Subject: [PATCH 5/5] fix global variable artifact --- nav2_motion_primitives/src/back_up.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_motion_primitives/src/back_up.cpp b/nav2_motion_primitives/src/back_up.cpp index dbd4c06cd6..c29891bb42 100644 --- a/nav2_motion_primitives/src/back_up.cpp +++ b/nav2_motion_primitives/src/back_up.cpp @@ -75,9 +75,9 @@ nav2_tasks::TaskStatus BackUp::controlledBackup() double diff_x = initial_pose_->pose.pose.position.x - current_odom_pose->pose.pose.position.x; double diff_y = initial_pose_->pose.pose.position.y - current_odom_pose->pose.pose.position.y; - double distance_ = sqrt(diff_x * diff_x + diff_y * diff_y); + double distance = sqrt(diff_x * diff_x + diff_y * diff_y); - if (distance_ >= abs(command_x_)) { + if (distance >= abs(command_x_)) { cmd_vel.linear.x = 0; robot_->sendVelocity(cmd_vel); return TaskStatus::SUCCEEDED;