Feature: Basic Robot Simulator

README.md

@@ -40,15 +40,24 @@ The library provides an easy templated API for testing basic communication nodes
 
 ## Features
 List the ready features here:
+
+### Asynchronous Communication
 - `Basic Subscriber`
 - `Basic Publisher`
+
+### Synchronous Communication
 - `Basic Service Client`
 - `Basic Action Client`
 - `Basic Action Server`
 - `Basic Service Server`
+
+### Spinners
 - `Single Threaded Spinner` - calls `spin_some` for all nodes registered in one different thread.
 - `Multi Threaded Spinner` - calls `spin_some` for groups. Implements multiple instances of `SingleThreadSpinner`
 
+### Robots
+- `SimulatedDifferentialRobot`
+
 
 ## Setup
 

include/cmr_tests_utils/base_rclpp_test_fixture.hpp

@@ -2,7 +2,7 @@
 #define BASE_RCLCPP_TEST_FIXTURE_HPP
 
 #include <gtest/gtest.h>
-#include "rclcpp/rclcpp.hpp"
+#include <rclcpp/rclcpp.hpp>
 
 namespace {
 
@@ -22,7 +22,6 @@ class BaseRclcppTestFixture : public ::testing::Test
  void SetUp() override {}
 
  void TearDown() override {}
-
 };
 
 }

include/cmr_tests_utils/basic_action_client_test.hpp

@@ -1,10 +1,11 @@
 #ifndef BASIC_ACTION_CLIENT_TEST_HPP
 #define BASIC_ACTION_CLIENT_TEST_HPP
 
-#include "rclcpp/rclcpp.hpp"
-#include "rclcpp_action/rclcpp_action.hpp"
 #include <chrono>
 
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+
 namespace cmr_tests_utils {
 
 template<class ActionT>

include/cmr_tests_utils/basic_action_server_test.hpp

@@ -4,8 +4,8 @@
 #include <string>
 #include <memory>
 
-#include "rclcpp/rclcpp.hpp"
-#include "rclcpp_action/rclcpp_action.hpp"
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
 
 namespace cmr_tests_utils {
 

include/cmr_tests_utils/basic_publisher_node_test.hpp

@@ -1,11 +1,12 @@
 #ifndef BASIC_PUBLISHER_NODE_TEST_HPP
 #define BASIC_PUBLISHER_NODE_TEST_HPP
 
-#include "rclcpp/rclcpp.hpp"
 #include <mutex>
 #include <chrono>
 #include <thread>
 
+#include <rclcpp/rclcpp.hpp>
+
 namespace cmr_tests_utils {
 
 template<class MessageT>

include/cmr_tests_utils/basic_service_client_test.hpp

@@ -1,9 +1,10 @@
 #ifndef BASIC_SERVICE_CLIENT_TEST_HPP
 #define BASIC_SERVICE_CLIENT_TEST_HPP
 
-#include "rclcpp/rclcpp.hpp"
 #include <chrono>
 
+#include <rclcpp/rclcpp.hpp>
+
 namespace cmr_tests_utils {
 
 template<class ServiceT>

include/cmr_tests_utils/basic_service_server_test.hpp

@@ -4,7 +4,7 @@
 #include <string>
 #include <memory>
 
-#include "rclcpp/rclcpp.hpp"
+#include <rclcpp/rclcpp.hpp>
 
 namespace cmr_tests_utils {
 
@@ -29,6 +29,7 @@ class BasicServiceServerTest: public rclcpp::Node
  }
 
  protected:
+
  virtual void request_callback(
  const std::shared_ptr<rmw_request_id_t> request_header,
  const std::shared_ptr<typename ServiceT::Request> request,
@@ -39,10 +40,11 @@ class BasicServiceServerTest: public rclcpp::Node
  last_request_ = request;
  }
 
+ std::shared_ptr<typename ServiceT::Request> last_request_;
+
  private:
 
  typename rclcpp::Service<ServiceT>::SharedPtr server_;
- std::shared_ptr<typename ServiceT::Request> last_request_;
 };
 
 }

include/cmr_tests_utils/basic_subscriber_node_test.hpp

@@ -1,9 +1,10 @@
 #ifndef BASIC_SUBSCRIBER_NODE_TEST_HPP
 #define BASIC_SUBSCRIBER_NODE_TEST_HPP
 
-#include "rclcpp/rclcpp.hpp"
 #include <mutex>
 
+#include <rclcpp/rclcpp.hpp>
+
 namespace cmr_tests_utils
 {
 

include/cmr_tests_utils/basic_tf_broadcaster_node_test.hpp

@@ -1,14 +1,14 @@
 #ifndef BASIC_TF_BROADCASTER_NODE_TEST_HPP
 #define BASIC_TF_BROADCASTER_NODE_TEST_HPP
 
-#include "rclcpp/rclcpp.hpp"
-#include "geometry_msgs/msg/transform_stamped.hpp"
-#include "geometry_msgs/msg/pose_stamped.hpp"
-#include "tf2_ros/transform_broadcaster.h"
-#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
-#include "tf2_ros/buffer.h"
-#include "tf2_ros/transform_listener.h"
-#include "tf2/utils.h"
+#include <rclcpp/rclcpp.hpp>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2/utils.h>
 
 namespace cmr_tests_utils
 {
@@ -18,16 +18,16 @@ class BasicTfBroadcasterNodeTest: public rclcpp::Node {
  public:
 
  BasicTfBroadcasterNodeTest(std::string node_name, geometry_msgs::msg::TransformStamped initial_transform, 
- unsigned int publish_period_ms): rclcpp::Node(node_name)
+ unsigned int publish_period_ms = 100, bool use_timer = true): rclcpp::Node(node_name)
  {
  clock_ = get_clock();
  buffer_ = std::make_shared<tf2_ros::Buffer>(clock_);
  listener_ = std::make_shared<tf2_ros::TransformListener>(*buffer_);
  broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
  transform_ = initial_transform;
- timer_ = this->create_wall_timer(
+ if (use_timer) timer_ = this->create_wall_timer(
  std::chrono::milliseconds(publish_period_ms), 
- std::bind(&BasicTfBroadcasterNodeTest::broadcast_transform_, this)
+ std::bind(&BasicTfBroadcasterNodeTest::broadcast_transform, this)
  );
  }
 
@@ -43,16 +43,15 @@ class BasicTfBroadcasterNodeTest: public rclcpp::Node {
  transform_ = transform;
  }
 
- private:
-
- void broadcast_transform_() 
+ void broadcast_transform() 
  {
- RCLCPP_ERROR(get_logger(), "Fuck you");
  std::lock_guard<std::mutex> lock(tf_mutex_);
  transform_.header.stamp = clock_->now();
  broadcaster_->sendTransform(transform_);
  }
 
+ private:
+
  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::Duration transform_tolerance_ {0, 0};
  std::shared_ptr<tf2_ros::Buffer> buffer_;

include/cmr_tests_utils/basic_tf_listener_node_test.hpp

@@ -1,16 +1,16 @@
 #ifndef BASIC_TF_LISTENER_NODE_TEST_HPP
 #define BASIC_TF_LISTENER_NODE_TEST_HPP
 
-#include "rclcpp/rclcpp.hpp"
-#include "tf2/LinearMath/Quaternion.h"
-#include "tf2/LinearMath/Transform.h"
-#include "geometry_msgs/msg/transform_stamped.hpp"
-#include "geometry_msgs/msg/pose_stamped.hpp"
-#include "tf2_ros/transform_broadcaster.h"
-#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
-#include "tf2_ros/buffer.h"
-#include "tf2_ros/transform_listener.h"
-#include "tf2/utils.h"
+#include <rclcpp/rclcpp.hpp>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2/LinearMath/Transform.h>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2/utils.h>
 
 namespace cmr_tests_utils
 {

include/cmr_tests_utils/multi_thread_spinner.hpp

@@ -1,9 +1,11 @@
 #ifndef MULTI_THREAD_SPINNER_HPP
 #define MULTI_THREAD_SPINNER_HPP
 
-#include "rclcpp/rclcpp.hpp"
 #include <map>
-#include "rclcpp_lifecycle/lifecycle_node.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_lifecycle/lifecycle_node.hpp>
+
 #include "cmr_tests_utils/single_thread_spinner.hpp"
 
 namespace cmr_tests_utils {

include/cmr_tests_utils/simulated_differential_robot.hpp

@@ -0,0 +1,131 @@
+#ifndef SIMULATED_DIFFERENTIAL_ROBOT
+#define SIMULATED_DIFFERENTIAL_ROBOT
+
+#include <chrono>
+
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <geometry_msgs/msg/twist.hpp>
+
+#include "cmr_tests_utils/basic_tf_broadcaster_node_test.hpp"
+#include "cmr_tests_utils/basic_subscriber_node_test.hpp"
+
+namespace cmr_tests_utils {
+
+class SimulatedDifferentialRobot: public rclcpp::Node 
+{
+ public: 
+
+ SimulatedDifferentialRobot(std::string node_name, std::string global_frame, std::string base_frame, std::string velocity_topic,
+ double velocity_timeout_sec = 0.5,
+ rclcpp::QoS velocity_topic_qos = rclcpp::SystemDefaultsQoS()): rclcpp::Node(node_name)
+ {
+ clock_ = get_clock();
+ buffer_ = std::make_shared<tf2_ros::Buffer>(clock_);
+ listener_ = std::make_shared<tf2_ros::TransformListener>(*buffer_);
+ broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
+ global_frame_ = global_frame;
+ base_frame_ = base_frame; 
+ velocity_timeout_sec_ = velocity_timeout_sec;
+
+ transform_.header.frame_id = global_frame;
+ transform_.child_frame_id = base_frame;
+
+ timer_ = this->create_wall_timer(
+ std::chrono::milliseconds(int(broadcast_period_sec_*1000)), 
+ std::bind(&SimulatedDifferentialRobot::broadcast_transform_, this)
+ ); 
+
+ vel_sub_ = this->create_subscription<geometry_msgs::msg::Twist> (
+ velocity_topic, velocity_topic_qos,
+ std::bind (&SimulatedDifferentialRobot::vel_callback_, this, std::placeholders::_1));
+ } 
+
+ bool has_vel_been_received() const 
+ {
+ return (last_received_vel_ != nullptr);
+ } 
+
+ const bool& is_broadcasting() const
+ {
+ return is_broadcasting_;
+ }
+
+ const geometry_msgs::msg::TransformStamped& get_transform () const
+ {
+ return transform_;
+ }
+
+ const std::shared_ptr<geometry_msgs::msg::Twist> get_last_velocity_msg() const
+ {
+ geometry_msgs::msg::Twist msg;
+ if (!last_received_vel_) 
+ {
+ RCLCPP_WARN(get_logger(), "Tried to get last received velocity from subscription but nothing was published yet.");
+ }
+ return last_received_vel_;
+ } 
+
+
+ private: 
+
+ void broadcast_transform_() 
+ {
+ is_broadcasting_ = true;
+
+ if (last_received_vel_)
+ {
+ std::chrono::duration<double> elapsed_time = std::chrono::system_clock::now() - last_velocity_timestamp_;
+
+ // Last received command was too long ago, the robot stops
+ if (elapsed_time.count() > velocity_timeout_sec_)
+ {
+ transform_.header.stamp = clock_->now();
+ broadcaster_->sendTransform(transform_);
+ return;
+ }
+
+ // Estimate pose using last command and dt since 
+ transform_.transform.translation.x += broadcast_period_sec_ * last_received_vel_->linear.x * cos(last_yaw_);
+ transform_.transform.translation.y += broadcast_period_sec_ * last_received_vel_->linear.y * sin(last_yaw_);
+ last_yaw_ += broadcast_period_sec_ * last_received_vel_->angular.z; 
+ tf2::Quaternion quat;
+ quat.setRPY(0, 0, last_yaw_);
+ transform_.transform.rotation = tf2::toMsg(quat);
+ }
+
+ transform_.header.stamp = clock_->now();
+ broadcaster_->sendTransform(transform_);
+ } 
+
+ void vel_callback_(const std::shared_ptr<geometry_msgs::msg::Twist> msg) 
+ {
+ last_received_vel_ = msg;
+ last_velocity_timestamp_ = std::chrono::system_clock::now();
+ }
+
+ // Transforms Utils
+ rclcpp::TimerBase::SharedPtr timer_;
+ geometry_msgs::msg::TransformStamped transform_;
+ rclcpp::Duration transform_tolerance_ {0, 0};
+ std::shared_ptr<tf2_ros::Buffer> buffer_;
+ std::shared_ptr<tf2_ros::TransformListener> listener_;
+ std::shared_ptr<tf2_ros::TransformBroadcaster> broadcaster_;
+ rclcpp::Clock::SharedPtr clock_; 
+
+ // Subscription Utils
+ typename rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr vel_sub_;
+ std::shared_ptr<geometry_msgs::msg::Twist> last_received_vel_;
+
+ // Internal Attributes
+ std::string base_frame_;
+ std::string global_frame_;
+ std::chrono::system_clock::time_point last_velocity_timestamp_; // As long as we don't have TwistStamped for velocities, it's the best we can do
+ bool is_broadcasting_ = false;
+ double last_yaw_ = 0.0; 
+ double velocity_timeout_sec_ = 0.5;
+ double broadcast_period_sec_ = 0.01; // 100 Hz
+};
+
+}
+
+#endif

include/cmr_tests_utils/single_thread_spinner.hpp

@@ -1,11 +1,12 @@
 #ifndef SINGLE_THREAD_SPINNER_HPP
 #define SINGLE_THREAD_SPINNER_HPP
 
-#include "rclcpp/rclcpp.hpp"
-#include "rclcpp_lifecycle/lifecycle_node.hpp"
 #include <thread>
 #include <map>
 
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_lifecycle/lifecycle_node.hpp>
+
 namespace cmr_tests_utils {
 
 class SingleThreadSpinner: private rclcpp::executors::SingleThreadedExecutor
@@ -129,7 +130,6 @@ class SingleThreadSpinner: private rclcpp::executors::SingleThreadedExecutor
  std::lock_guard<std::mutex> lock(mutex_);
  rclcpp::executors::SingleThreadedExecutor::spin_some();
  }
- std::this_thread::sleep_for(std::chrono::milliseconds(30));
  if (cancel_spin_called_.load()) break;
  }
  RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Single Thread Spinner was cancelled.");