Heavy cleanup of the draw_square tutorial.

turtlesim/tutorials/draw_square.cpp

@@ -1,168 +1,210 @@
-#include <rclcpp/rclcpp.hpp>
-#include <turtlesim/msg/pose.hpp>
+/*
+ * Copyright (c) 2009, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <chrono>
+#include <cmath>
+#include <functional>
+#include <memory>
+
 #include <geometry_msgs/msg/twist.hpp>
+#include <rclcpp/rclcpp.hpp>
 #include <std_srvs/srv/empty.hpp>
-#include <math.h>
-
-turtlesim::msg::Pose g_pose;
-turtlesim::msg::Pose g_goal;
-
-enum State
-{
- FORWARD,
- STOP_FORWARD,
- TURN,
- STOP_TURN,
-};
-
-State g_state = FORWARD;
-State g_last_state = FORWARD;
-bool g_first_goal_set = false;
-bool g_first_pose_set = false;
+#include <turtlesim/msg/pose.hpp>
 
-#define PI 3.141592
+#define PI 3.141592f
 
-void poseCallback(const turtlesim::msg::Pose & pose)
+class DrawSquare final : public rclcpp::Node
 {
- g_pose = pose;
- if (!g_first_pose_set)
+public:
+ explicit DrawSquare(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
+ : rclcpp::Node("draw_square", options)
  {
- g_first_pose_set = true;
- }
-}
+ twist_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("turtle1/cmd_vel", 1);
 
-bool hasReachedGoal()
-{
- return fabsf(g_pose.x - g_goal.x) < 0.1 && fabsf(g_pose.y - g_goal.y) < 0.1 && fabsf(g_pose.theta - g_goal.theta) < 0.01;
-}
-
-bool hasStopped()
-{
- return g_pose.angular_velocity < 0.0001 && g_pose.linear_velocity < 0.0001;
-}
+ pose_sub_ =
+ this->create_subscription<turtlesim::msg::Pose>(
+ "turtle1/pose", 1, std::bind(&DrawSquare::poseCallback, this, std::placeholders::_1));
 
-void printGoal()
-{
- RCLCPP_INFO(rclcpp::get_logger("draw_square"), "New goal [%f %f, %f]", g_goal.x, g_goal.y, g_goal.theta);
-}
+ reset_client_ = this->create_client<std_srvs::srv::Empty>("reset");
 
-void commandTurtle(rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr twist_pub, float linear, float angular)
-{
- geometry_msgs::msg::Twist twist;
- twist.linear.x = linear;
- twist.angular.z = angular;
- twist_pub->publish(twist);
-}
+ timer_ = this->create_wall_timer(std::chrono::milliseconds(16), [this]() {timerCallback();});
 
-void stopForward(rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr twist_pub)
-{
- if (hasStopped())
- {
- RCLCPP_INFO(rclcpp::get_logger("draw_square"), "Reached goal");
- g_state = TURN;
- g_goal.x = g_pose.x;
- g_goal.y = g_pose.y;
- g_goal.theta = fmod(g_pose.theta + static_cast<float>(PI) / 2.0f, 2.0f * static_cast<float>(PI));
- // wrap g_goal.theta to [-pi, pi)
- if (g_goal.theta >= static_cast<float>(PI)) g_goal.theta -= 2.0f * static_cast<float>(PI);
- printGoal();
+ auto empty = std::make_shared<std_srvs::srv::Empty::Request>();
+ reset_result_ = reset_client_->async_send_request(empty).future;
  }
- else
- {
- commandTurtle(twist_pub, 0, 0);
- }
-}
 
-void stopTurn(rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr twist_pub)
-{
- if (hasStopped())
+private:
+ enum State
  {
- RCLCPP_INFO(rclcpp::get_logger("draw_square"), "Reached goal");
- g_state = FORWARD;
- g_goal.x = cos(g_pose.theta) * 2 + g_pose.x;
- g_goal.y = sin(g_pose.theta) * 2 + g_pose.y;
- g_goal.theta = g_pose.theta;
- printGoal();
- }
- else
- {
- commandTurtle(twist_pub, 0, 0);
- }
-}
-
+ FORWARD,
+ STOP_FORWARD,
+ TURN,
+ STOP_TURN,
+ };
 
-void forward(rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr twist_pub)
-{
- if (hasReachedGoal())
+ void poseCallback(const turtlesim::msg::Pose & pose)
  {
- g_state = STOP_FORWARD;
- commandTurtle(twist_pub, 0, 0);
+ current_pose_ = pose;
+ first_pose_set_ = true;
  }
- else
+
+ bool hasReachedGoal()
  {
- commandTurtle(twist_pub, 1.0f, 0);
+ return fabsf(current_pose_.x - goal_pose_.x) < 0.1 &&
+ fabsf(current_pose_.y - goal_pose_.y) < 0.1 &&
+ fabsf(current_pose_.theta - goal_pose_.theta) < 0.01;
  }
-}
 
-void turn(rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr twist_pub)
-{
- if (hasReachedGoal())
+ bool hasStopped()
  {
- g_state = STOP_TURN;
- commandTurtle(twist_pub, 0, 0);
+ return current_pose_.angular_velocity < 0.0001 && current_pose_.linear_velocity < 0.0001;
  }
- else
+
+ void printGoal()
  {
- commandTurtle(twist_pub, 0, 0.4f);
+ RCLCPP_INFO(
+ this->get_logger(), "New goal [%f %f, %f]", goal_pose_.x, goal_pose_.y, goal_pose_.theta);
  }
-}
 
-void timerCallback(rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr twist_pub)
-{
- if (g_first_pose_set)
+ void commandTurtle(float linear, float angular)
  {
- return;
+ geometry_msgs::msg::Twist twist;
+ twist.linear.x = linear;
+ twist.angular.z = angular;
+ twist_pub_->publish(twist);
  }
 
- if (!g_first_goal_set)
+ void stopForward()
  {
- g_first_goal_set = true;
- g_state = FORWARD;
- g_goal.x = cos(g_pose.theta) * 2 + g_pose.x;
- g_goal.y = sin(g_pose.theta) * 2 + g_pose.y;
- g_goal.theta = g_pose.theta;
- printGoal();
+ if (hasStopped()) {
+ RCLCPP_INFO(this->get_logger(), "Reached goal");
+ state_ = TURN;
+ goal_pose_.x = current_pose_.x;
+ goal_pose_.y = current_pose_.y;
+ goal_pose_.theta = fmod(current_pose_.theta + PI / 2.0f, 2.0f * PI);
+ // wrap goal_pose_.theta to [-pi, pi)
+ if (goal_pose_.theta >= PI) {
+ goal_pose_.theta -= 2.0f * PI;
+ }
+ printGoal();
+ } else {
+ commandTurtle(0, 0);
+ }
  }
 
- if (g_state == FORWARD)
+ void stopTurn()
  {
- forward(twist_pub);
+ if (hasStopped()) {
+ RCLCPP_INFO(this->get_logger(), "Reached goal");
+ state_ = FORWARD;
+ goal_pose_.x = cos(current_pose_.theta) * 2 + current_pose_.x;
+ goal_pose_.y = sin(current_pose_.theta) * 2 + current_pose_.y;
+ goal_pose_.theta = current_pose_.theta;
+ printGoal();
+ } else {
+ commandTurtle(0, 0);
+ }
  }
- else if (g_state == STOP_FORWARD)
+
+ void forward()
  {
- stopForward(twist_pub);
+ if (hasReachedGoal()) {
+ state_ = STOP_FORWARD;
+ commandTurtle(0, 0);
+ } else {
+ commandTurtle(1.0f, 0);
+ }
  }
- else if (g_state == TURN)
+
+ void turn()
  {
- turn(twist_pub);
+ if (hasReachedGoal()) {
+ state_ = STOP_TURN;
+ commandTurtle(0, 0);
+ } else {
+ commandTurtle(0, 0.4f);
+ }
  }
- else if (g_state == STOP_TURN)
+
+ void timerCallback()
  {
- stopTurn(twist_pub);
+ if (!reset_result_.valid()) {
+ return;
+ }
+
+ if (!first_pose_set_) {
+ return;
+ }
+
+ if (!first_goal_set_) {
+ first_goal_set_ = true;
+ state_ = FORWARD;
+ goal_pose_.x = cos(current_pose_.theta) * 2 + current_pose_.x;
+ goal_pose_.y = sin(current_pose_.theta) * 2 + current_pose_.y;
+ goal_pose_.theta = current_pose_.theta;
+ printGoal();
+ }
+
+ if (state_ == FORWARD) {
+ forward();
+ } else if (state_ == STOP_FORWARD) {
+ stopForward();
+ } else if (state_ == TURN) {
+ turn();
+ } else if (state_ == STOP_TURN) {
+ stopTurn();
+ }
  }
-}
 
-int main(int argc, char** argv)
+ turtlesim::msg::Pose current_pose_;
+ turtlesim::msg::Pose goal_pose_;
+ bool first_goal_set_ = false;
+ bool first_pose_set_ = false;
+ State state_ = FORWARD;
+ State last_state_ = FORWARD;
+
+ rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr twist_pub_;
+ rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr pose_sub_;
+ rclcpp::Client<std_srvs::srv::Empty>::SharedPtr reset_client_;
+ rclcpp::Client<std_srvs::srv::Empty>::SharedFuture reset_result_;
+ rclcpp::TimerBase::SharedPtr timer_;
+};
+
+int main(int argc, char ** argv)
 {
  rclcpp::init(argc, argv);
- auto nh = rclcpp::Node::make_shared("draw_square");
- auto pose_sub = nh->create_subscription<turtlesim::msg::Pose>("turtle1/pose", 1, std::bind(poseCallback, std::placeholders::_1));
- auto twist_pub = nh->create_publisher<geometry_msgs::msg::Twist>("turtle1/cmd_vel", 1);
- auto reset = nh->create_client<std_srvs::srv::Empty>("reset");
- auto timer = nh->create_wall_timer(std::chrono::milliseconds(16), [twist_pub](){timerCallback(twist_pub);});
 
- auto empty = std::make_shared<std_srvs::srv::Empty::Request>();
- reset->async_send_request(empty);
+ auto nh = std::make_shared<DrawSquare>();
 
  rclcpp::spin(nh);
+
+ rclcpp::shutdown();
+
+ return 0;
 }