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

@@ -22,7 +22,6 @@ class BaseRclcppTestFixture : public ::testing::Test
  void SetUp() override {}
 
  void TearDown() override {}
-
 };
 
 }

include/cmr_tests_utils/basic_service_server_test.hpp

@@ -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_tf_broadcaster_node_test.hpp

@@ -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/simulated_differential_robot.hpp

@@ -0,0 +1,130 @@
+#ifndef SIMULATED_DIFFERENTIAL_ROBOT
+#define SIMULATED_DIFFERENTIAL_ROBOT
+
+#include <chrono>
+#include "cmr_tests_utils/basic_tf_broadcaster_node_test.hpp"
+#include "cmr_tests_utils/basic_subscriber_node_test.hpp"
+#include "geometry_msgs/msg/transform_stamped.hpp"
+#include "geometry_msgs/msg/twist.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_;
+ }
+
+ 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 msg;
+ }
+ 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

@@ -129,7 +129,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.");

test/CMakeLists.txt

@@ -26,4 +26,10 @@ set(TEST_NAME transforms_test)
 ament_add_gtest(${TEST_NAME} ${TEST_NAME}.cpp)
 target_link_libraries(${TEST_NAME} ${LIBRARY_NAME})
 ament_target_dependencies(${TEST_NAME} ${dependencies})
+target_include_directories(${TEST_NAME} PRIVATE "../include" "include")
+
+set(TEST_NAME simulated_robot_test)
+ament_add_gtest(${TEST_NAME} ${TEST_NAME}.cpp)
+target_link_libraries(${TEST_NAME} ${LIBRARY_NAME})
+ament_target_dependencies(${TEST_NAME} ${dependencies})
 target_include_directories(${TEST_NAME} PRIVATE "../include" "include")

test/simulated_robot_test.cpp

@@ -0,0 +1,119 @@
+#include "gtest/gtest.h"
+#include "rclcpp/rclcpp.hpp"
+#include "cmr_tests_utils/basic_publisher_node_test.hpp"
+#include "cmr_tests_utils/simulated_differential_robot.hpp"
+#include "cmr_tests_utils/basic_tf_listener_node_test.hpp"
+#include "cmr_tests_utils/multi_thread_spinner.hpp"
+#include "geometry_msgs/msg/transform_stamped.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+#include <chrono>
+#include <thread>
+
+TEST(SimulatedRobot, robot_has_not_moved)
+{
+ rclcpp::init(0, nullptr);
+
+ auto spinner = cmr_tests_utils::MultiThreadSpinner();
+ auto tf_listener = std::make_shared<cmr_tests_utils::BasicTfListenerNodeTest>("tf_listener_node", 1.0);
+ auto robot = std::make_shared<cmr_tests_utils::SimulatedDifferentialRobot>("robot_node", "odom", "base_footprint", "cmd_vel");
+
+ spinner.add_node(tf_listener->get_node_base_interface());
+ spinner.add_node(robot->get_node_base_interface());
+
+ ASSERT_TRUE(spinner.spin_all_nodes());
+
+ // Test if robot has been constructed correctly
+ ASSERT_FALSE(robot->has_vel_been_received());
+ ASSERT_EQ(robot->get_transform().header.frame_id, "odom");
+ ASSERT_EQ(robot->get_transform().child_frame_id, "base_footprint");
+
+ std::this_thread::sleep_for(std::chrono::milliseconds(50));
+ ASSERT_TRUE(robot->is_broadcasting());
+
+ // Test if external nodes can access the Robot's state
+ geometry_msgs::msg::TransformStamped test_tf;
+ ASSERT_TRUE(tf_listener->lookup_transform(test_tf, "odom", "base_footprint"));
+ ASSERT_EQ(test_tf.transform.translation.x, 0);
+ geometry_msgs::msg::PoseStamped source, target, pose;
+ source.header.frame_id = "odom";
+ source.pose.position.x = 5;
+ ASSERT_TRUE(tf_listener->transform_pose("base_footprint", source, target));
+ ASSERT_EQ(target.pose.position.x, 5);
+
+ rclcpp::shutdown();
+}
+
+TEST(SimulatedRobot, robot_receives_exactly_one_command)
+{
+ rclcpp::init(0, nullptr);
+
+ auto spinner = cmr_tests_utils::MultiThreadSpinner();
+ auto robot_spinner = cmr_tests_utils::SingleThreadSpinner();
+ auto velocity_pub = std::make_shared<cmr_tests_utils::BasicPublisherNodeTest<geometry_msgs::msg::Twist>>("velocity_commander_node", "cmd_vel", false);
+ auto tf_listener = std::make_shared<cmr_tests_utils::BasicTfListenerNodeTest>("tf_listener_node", 1.0);
+ auto robot = std::make_shared<cmr_tests_utils::SimulatedDifferentialRobot>("robot_node", "odom", "base_footprint", "cmd_vel");
+
+ spinner.add_node(velocity_pub->get_node_base_interface());
+ spinner.add_node(tf_listener->get_node_base_interface());
+ spinner.add_node(robot->get_node_base_interface());
+
+ ASSERT_TRUE(spinner.spin_all_nodes());
+
+ // Test if robot has been constructed correctly
+ ASSERT_FALSE(robot->has_vel_been_received());
+ ASSERT_EQ(robot->get_transform().header.frame_id, "odom");
+ ASSERT_EQ(robot->get_transform().child_frame_id, "base_footprint");
+
+ std::this_thread::sleep_for(std::chrono::milliseconds(150));
+ ASSERT_TRUE(robot->is_broadcasting());
+
+ // Test if external nodes can access the Robot's state
+ geometry_msgs::msg::TransformStamped test_tf;
+ ASSERT_TRUE(tf_listener->lookup_transform(test_tf, "odom", "base_footprint"));
+ ASSERT_EQ(test_tf.transform.translation.x, 0);
+ geometry_msgs::msg::PoseStamped source, target, pose;
+ source.header.frame_id = "odom";
+ source.pose.position.x = 5;
+ ASSERT_TRUE(tf_listener->transform_pose("base_footprint", source, target));
+ ASSERT_EQ(target.pose.position.x, 5);
+
+ // Publish exactly one velocity
+ geometry_msgs::msg::Twist twist;
+ twist.linear.x = 1.0;
+ velocity_pub->publish(twist);
+
+ // Wait for the robot to move
+ std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+
+ // The robot should've moved at 1.0 m/s for 0.5s, it should be 0.5m forward
+ ASSERT_TRUE(robot->has_vel_been_received());
+ ASSERT_EQ(robot->get_transform().header.frame_id, "odom");
+ ASSERT_EQ(robot->get_transform().child_frame_id, "base_footprint");
+ ASSERT_NEAR(robot->get_transform().transform.translation.x, 0.5, 0.01);
+ ASSERT_NEAR(robot->get_transform().transform.translation.y, 0.0, 0.01);
+ ASSERT_NEAR(robot->get_transform().transform.rotation.x, 0.0, 0.01);
+ ASSERT_NEAR(robot->get_transform().transform.rotation.y, 0.0, 0.01);
+ ASSERT_NEAR(robot->get_transform().transform.rotation.z, 0.0, 0.01);
+ ASSERT_NEAR(robot->get_transform().transform.rotation.w, 1.0, 0.01);
+
+ // Send new command
+ twist.linear.x = 0;
+ twist.angular.z = 3.14;
+ velocity_pub->publish(twist);
+
+ // Wait for the robot to move
+ std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+
+ ASSERT_TRUE(robot->has_vel_been_received());
+ ASSERT_EQ(robot->get_transform().header.frame_id, "odom");
+ ASSERT_EQ(robot->get_transform().child_frame_id, "base_footprint");
+ ASSERT_NEAR(robot->get_transform().transform.translation.x, 0.5, 0.01);
+ ASSERT_NEAR(robot->get_transform().transform.translation.y, 0.0, 0.01);
+ ASSERT_NEAR(robot->get_transform().transform.rotation.x, 0.0, 0.01);
+ ASSERT_NEAR(robot->get_transform().transform.rotation.y, 0.0, 0.01);
+ ASSERT_NEAR(robot->get_transform().transform.rotation.z, 0.707, 0.01);
+ ASSERT_NEAR(robot->get_transform().transform.rotation.w, 0.707, 0.01);
+
+ rclcpp::shutdown();
+}