Add RotateAbsolute action to turtlesim (ros#62)

* [turtlesim] Add action interface for rotating to absolute angle

Signed-off-by: Jacob Perron <[email protected]>

* Implement RotateAbsolute action

Only one action is executed at a time. Any new actions override existing ones.
If a velocity command is received while executing the action, then the goal is aborted.

Signed-off-by: Jacob Perron <[email protected]>

* Support triggering rotation action with keyboard teleop

Use the keys g/b/v/c/d/e/r/t to rotate the turtle to absolute orientations and 'f' to cancel a rotation.

Signed-off-by: Jacob Perron <[email protected]>

* Catch exceptions when attempting to cancel goal

Signed-off-by: Jacob Perron <[email protected]>

* Add back input.shutdown()

Signed-off-by: Jacob Perron <[email protected]>

* Add back rclcpp::shutdown

Signed-off-by: Jacob Perron <[email protected]>

* Define TWO_PI from PI in header

Signed-off-by: Jacob Perron <[email protected]>

* match style

Author: jacobperron

turtlesim/CMakeLists.txt

@@ -14,13 +14,15 @@ find_package(ament_index_cpp REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(Qt5 REQUIRED COMPONENTS Widgets)
 find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(std_srvs REQUIRED)
 
 include_directories(include ${Qt5Widgets_INCLUDE_DIRS})
 
 rosidl_generate_interfaces(${PROJECT_NAME}
+  "action/RotateAbsolute.action"
   "msg/Color.msg"
   "msg/Pose.msg"
   "srv/Kill.srv"
@@ -29,7 +31,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/TeleportAbsolute.srv"
   "srv/TeleportRelative.srv")
 
-set(dependencies "rclcpp" "ament_index_cpp" "geometry_msgs" "std_msgs" "std_srvs")
+set(dependencies "ament_index_cpp" "geometry_msgs" "rclcpp" "rclcpp_action" "std_msgs" "std_srvs")
 
 set(turtlesim_node_SRCS
   src/turtlesim.cpp
@@ -50,6 +52,7 @@ rosidl_target_interfaces(turtlesim_node ${PROJECT_NAME} "rosidl_typesupport_cpp"
 
 add_executable(turtle_teleop_key tutorials/teleop_turtle_key.cpp)
 ament_target_dependencies(turtle_teleop_key ${dependencies})
+rosidl_target_interfaces(turtle_teleop_key ${PROJECT_NAME} "rosidl_typesupport_cpp")
 add_executable(draw_square tutorials/draw_square.cpp)
 ament_target_dependencies(draw_square ${dependencies})
 rosidl_target_interfaces(draw_square ${PROJECT_NAME} "rosidl_typesupport_cpp")

turtlesim/action/RotateAbsolute.action

@@ -0,0 +1,8 @@
+# The desired heading in radians
+float32 theta
+---
+# The angular displacement in radians to the starting position
+float32 delta
+---
+# The remaining rotation in radians
+float32 remaining

turtlesim/include/turtlesim/turtle.h

@@ -33,8 +33,10 @@
 // This prevents a MOC error with versions of boost >= 1.48
 #ifndef Q_MOC_RUN  // See: https://bugreports.qt-project.org/browse/QTBUG-22829
 # include <rclcpp/rclcpp.hpp>
+# include <rclcpp_action/rclcpp_action.hpp>
 
 # include <geometry_msgs/msg/twist.hpp>
+# include <turtlesim/action/rotate_absolute.hpp>
 # include <turtlesim/msg/color.hpp>
 # include <turtlesim/msg/pose.hpp>
 # include <turtlesim/srv/set_pen.hpp>
@@ -48,13 +50,16 @@
 #include <QPointF>
 
 #define PI 3.14159265
+#define TWO_PI 2.0 * PI
 
 namespace turtlesim
 {
 
 class Turtle
 {
 public:
+  using RotateAbsoluteGoalHandle = rclcpp_action::ServerGoalHandle<turtlesim::action::RotateAbsolute>;
+
   Turtle(rclcpp::Node::SharedPtr& nh, const std::string& real_name, const QImage& turtle_image, const QPointF& pos, float orient);
 
   bool update(double dt, QPainter& path_painter, const QImage& path_image, qreal canvas_width, qreal canvas_height);
@@ -64,6 +69,7 @@ class Turtle
   bool setPenCallback(const turtlesim::srv::SetPen::Request::SharedPtr, turtlesim::srv::SetPen::Response::SharedPtr);
   bool teleportRelativeCallback(const turtlesim::srv::TeleportRelative::Request::SharedPtr, turtlesim::srv::TeleportRelative::Response::SharedPtr);
   bool teleportAbsoluteCallback(const turtlesim::srv::TeleportAbsolute::Request::SharedPtr, turtlesim::srv::TeleportAbsolute::Response::SharedPtr);
+  void rotateAbsoluteAcceptCallback(const std::shared_ptr<RotateAbsoluteGoalHandle>);
 
   void rotateImage();
 
@@ -86,6 +92,12 @@ class Turtle
   rclcpp::Service<turtlesim::srv::SetPen>::SharedPtr set_pen_srv_;
   rclcpp::Service<turtlesim::srv::TeleportRelative>::SharedPtr teleport_relative_srv_;
   rclcpp::Service<turtlesim::srv::TeleportAbsolute>::SharedPtr teleport_absolute_srv_;
+  rclcpp_action::Server<turtlesim::action::RotateAbsolute>::SharedPtr rotate_absolute_action_server_;
+
+  std::shared_ptr<RotateAbsoluteGoalHandle> rotate_absolute_goal_handle_;
+  std::shared_ptr<turtlesim::action::RotateAbsolute::Feedback> rotate_absolute_feedback_;
+  std::shared_ptr<turtlesim::action::RotateAbsolute::Result> rotate_absolute_result_;
+  qreal rotate_absolute_start_orient_;
 
   rclcpp::Time last_command_time_;
 

turtlesim/package.xml

@@ -27,6 +27,7 @@
   <depend>ament_index_cpp</depend>
   <depend>geometry_msgs</depend>
   <depend>rclcpp</depend>
+  <depend>rclcpp_action</depend>
   <depend>std_msgs</depend>
   <depend>std_srvs</depend>
 

turtlesim/src/turtle.cpp

@@ -41,6 +41,11 @@
 namespace turtlesim
 {
 
+static float normalizeAngle(float angle)
+{
+  return angle - (TWO_PI * std::floor((angle + PI) / (TWO_PI)));
+}
+
 Turtle::Turtle(rclcpp::Node::SharedPtr& nh, const std::string& real_name, const QImage& turtle_image, const QPointF& pos, float orient)
 : nh_(nh)
 , turtle_image_(turtle_image)
@@ -60,6 +65,20 @@ Turtle::Turtle(rclcpp::Node::SharedPtr& nh, const std::string& real_name, const
   set_pen_srv_ = nh_->create_service<turtlesim::srv::SetPen>(real_name + "/set_pen", std::bind(&Turtle::setPenCallback, this, std::placeholders::_1, std::placeholders::_2));
   teleport_relative_srv_ = nh_->create_service<turtlesim::srv::TeleportRelative>(real_name + "/teleport_relative", std::bind(&Turtle::teleportRelativeCallback, this, std::placeholders::_1, std::placeholders::_2));
   teleport_absolute_srv_ = nh_->create_service<turtlesim::srv::TeleportAbsolute>(real_name + "/teleport_absolute", std::bind(&Turtle::teleportAbsoluteCallback, this, std::placeholders::_1, std::placeholders::_2));
+  rotate_absolute_action_server_ = rclcpp_action::create_server<turtlesim::action::RotateAbsolute>(
+    nh,
+    real_name + "/rotate_absolute",
+    [](const rclcpp_action::GoalUUID &, std::shared_ptr<const turtlesim::action::RotateAbsolute::Goal>)
+    {
+      // Accept all goals
+      return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+    },
+    [](const std::shared_ptr<RotateAbsoluteGoalHandle>)
+    {
+      // Accept all cancel requests
+      return rclcpp_action::CancelResponse::ACCEPT;
+    },
+    std::bind(&Turtle::rotateAbsoluteAcceptCallback, this, std::placeholders::_1));
 
   last_command_time_ = nh_->now();
 
@@ -73,6 +92,14 @@ void Turtle::velocityCallback(const geometry_msgs::msg::Twist::SharedPtr vel)
   last_command_time_ = nh_->now();
   lin_vel_ = vel->linear.x;
   ang_vel_ = vel->angular.z;
+
+  // Abort any active action
+  if (rotate_absolute_goal_handle_)
+  {
+    RCLCPP_WARN(nh_->get_logger(), "Velocity command received during rotation action. Aborting action");
+    rotate_absolute_goal_handle_->abort(rotate_absolute_result_);
+    rotate_absolute_goal_handle_ = nullptr;
+  }
 }
 
 bool Turtle::setPenCallback(const turtlesim::srv::SetPen::Request::SharedPtr req, turtlesim::srv::SetPen::Response::SharedPtr)
@@ -105,6 +132,20 @@ bool Turtle::teleportAbsoluteCallback(const turtlesim::srv::TeleportAbsolute::Re
   return true;
 }
 
+void Turtle::rotateAbsoluteAcceptCallback(const std::shared_ptr<RotateAbsoluteGoalHandle> goal_handle)
+{
+  // Abort any existing goal
+  if (rotate_absolute_goal_handle_)
+  {
+    RCLCPP_WARN(nh_->get_logger(), "Rotation action received before a previous action finished. Aborting previous action");
+    rotate_absolute_goal_handle_->abort(rotate_absolute_result_);
+  }
+  rotate_absolute_goal_handle_ = goal_handle;
+  rotate_absolute_feedback_.reset(new turtlesim::action::RotateAbsolute::Feedback);
+  rotate_absolute_result_.reset(new turtlesim::action::RotateAbsolute::Result);
+  rotate_absolute_start_orient_ = orient_;
+}
+
 void Turtle::rotateImage()
 {
   QTransform transform;
@@ -148,6 +189,47 @@ bool Turtle::update(double dt, QPainter& path_painter, const QImage& path_image,
 
   teleport_requests_.clear();
 
+  // Process any action requests
+  if (rotate_absolute_goal_handle_)
+  {
+    // Check if there was a cancel request
+    if (rotate_absolute_goal_handle_->is_canceling())
+    {
+      RCLCPP_INFO(nh_->get_logger(), "Rotation action canceled");
+      rotate_absolute_goal_handle_->canceled(rotate_absolute_result_);
+      rotate_absolute_goal_handle_ = nullptr;
+      lin_vel_ = 0.0;
+      ang_vel_ = 0.0;
+    }
+    else
+    {
+      float theta = normalizeAngle(rotate_absolute_goal_handle_->get_goal()->theta);
+      float remaining = normalizeAngle(theta - static_cast<float>(orient_));
+
+      // Update result
+      rotate_absolute_result_->delta = normalizeAngle(static_cast<float>(rotate_absolute_start_orient_ - orient_));
+
+      // Update feedback
+      rotate_absolute_feedback_->remaining = remaining;
+      rotate_absolute_goal_handle_->publish_feedback(rotate_absolute_feedback_);
+
+      // Check stopping condition
+      if (fabs(normalizeAngle(static_cast<float>(orient_) - theta)) < 0.02)
+      {
+        rotate_absolute_goal_handle_->succeed(rotate_absolute_result_);
+        rotate_absolute_goal_handle_ = nullptr;
+        lin_vel_ = 0.0;
+        ang_vel_ = 0.0;
+      }
+      else
+      {
+        lin_vel_ = 0.0;
+        ang_vel_ = remaining < 0.0 ? -1.0 : 1.0;
+        last_command_time_ = nh_->now();
+      }
+    }
+  }
+
   if (nh_->now() - last_command_time_ > rclcpp::Duration(1.0, 0))
   {
     lin_vel_ = 0.0;
@@ -158,7 +240,7 @@ bool Turtle::update(double dt, QPainter& path_painter, const QImage& path_image,
 
   orient_ = orient_ + ang_vel_ * dt;
   // Keep orient_ between -pi and +pi
-  orient_ -= 2*PI * std::floor((orient_ + PI)/(2*PI));
+  orient_ = normalizeAngle(orient_);
   pos_.rx() += std::cos(orient_) * lin_vel_ * dt;
   pos_.ry() += - std::sin(orient_) * lin_vel_ * dt;