Merge pull request #1 from Karsten1987/ros2_master_api

Ros2 master api

ros_controllers/include/ros_controllers/trajectory.hpp

@@ -28,9 +28,9 @@ namespace ros_controllers
 {
 
 using TrajectoryPointIter =
-    std::vector<trajectory_msgs::msg::JointTrajectoryPoint>::iterator;
+  std::vector<trajectory_msgs::msg::JointTrajectoryPoint>::iterator;
 using TrajectoryPointConstIter =
-    std::vector<trajectory_msgs::msg::JointTrajectoryPoint>::const_iterator;
+  std::vector<trajectory_msgs::msg::JointTrajectoryPoint>::const_iterator;
 
 class Trajectory
 {

ros_controllers/src/joint_state_controller.cpp

@@ -72,7 +72,7 @@ JointStateController::update()
     return hardware_interface::HW_RET_ERROR;
   }
 
-  joint_state_msg_->header.stamp = rclcpp::Time::now();
+  joint_state_msg_->header.stamp = rclcpp::Clock().now();
   size_t i = 0;
   for (auto joint_state_handle : registered_joint_handles_) {
     joint_state_msg_->position[i] = joint_state_handle->get_position();
@@ -88,7 +88,7 @@ JointStateController::update()
 
 }  // namespace ros_controllers
 
-#include "class_loader/class_loader_register_macro.h"
+#include "class_loader/register_macro.hpp"
 
 CLASS_LOADER_REGISTER_CLASS(
   ros_controllers::JointStateController, controller_interface::ControllerInterface)

ros_controllers/src/joint_trajectory_controller.cpp

@@ -199,7 +199,7 @@ JointTrajectoryController::on_configure(const rclcpp_lifecycle::State & previous
   // subscriber call back
   // non realtime
   // TODO(karsten): check if traj msg and point time are valid
-  auto callback = [this](const typename trajectory_msgs::msg::JointTrajectory::SharedPtr msg)
+  auto callback = [this](const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> msg)
     -> void
     {
       if (registered_joint_cmd_handles_.size() != msg->joint_names.size()) {
@@ -330,7 +330,7 @@ JointTrajectoryController::update()
   }
 
   // find next new point for current timestamp
-  auto traj_point_ptr = (*traj_point_active_ptr_)->sample(rclcpp::Time::now());
+  auto traj_point_ptr = (*traj_point_active_ptr_)->sample(rclcpp::Clock().now());
   // find next new point for current timestamp
   // set cmd only if a point is found
   if (traj_point_ptr == (*traj_point_active_ptr_)->end()) {
@@ -374,7 +374,7 @@ JointTrajectoryController::halt()
 
 }  // namespace ros_controllers
 
-#include "class_loader/class_loader_register_macro.h"
+#include "class_loader/register_macro.hpp"
 
 CLASS_LOADER_REGISTER_CLASS(
   ros_controllers::JointTrajectoryController, controller_interface::ControllerInterface)

ros_controllers/src/trajectory.cpp

@@ -19,6 +19,8 @@
 #include "hardware_interface/macros.hpp"
 #include "hardware_interface/utils/time_utils.hpp"
 
+#include "rclcpp/clock.hpp"
+
 namespace ros_controllers
 {
 
@@ -35,7 +37,7 @@ Trajectory::Trajectory()
 Trajectory::Trajectory(std::shared_ptr<trajectory_msgs::msg::JointTrajectory> joint_trajectory)
 : trajectory_msg_(joint_trajectory),
   trajectory_start_time_(time_is_zero(joint_trajectory->header.stamp) ?
-    rclcpp::Time::now() :
+    rclcpp::Clock().now() :
     static_cast<rclcpp::Time>(joint_trajectory->header.stamp))
 {}
 
@@ -44,7 +46,7 @@ Trajectory::update(std::shared_ptr<trajectory_msgs::msg::JointTrajectory> joint_
 {
   trajectory_msg_ = joint_trajectory;
   trajectory_start_time_ = (time_is_zero(joint_trajectory->header.stamp) ?
-    rclcpp::Time::now() :
+    rclcpp::Clock().now() :
     static_cast<rclcpp::Time>(joint_trajectory->header.stamp));
 }
 

ros_controllers/test/test_trajectory.cpp

@@ -19,6 +19,8 @@
 
 #include "gtest/gtest.h"
 
+#include "rclcpp/clock.hpp"
+
 #include "ros_controllers/trajectory.hpp"
 
 using namespace std::chrono_literals;
@@ -38,18 +40,18 @@ TEST_F(TestTrajectory, initialize_trajectory) {
     empty_msg->header.stamp.nanosec = 2;
     rclcpp::Time empty_time = empty_msg->header.stamp;
     auto traj = ros_controllers::Trajectory(empty_msg);
-    EXPECT_EQ(traj.end(), traj.sample(rclcpp::Time::now()));
+    EXPECT_EQ(traj.end(), traj.sample(rclcpp::Clock().now()));
     EXPECT_EQ(empty_time, traj.time_from_start());
   }
   {
     auto empty_msg = std::make_shared<trajectory_msgs::msg::JointTrajectory>();
     empty_msg->header.stamp.sec = 0;
     empty_msg->header.stamp.nanosec = 0;
-    auto now = rclcpp::Time::now();
+    auto now = rclcpp::Clock().now();
     auto traj = ros_controllers::Trajectory(empty_msg);
     auto traj_starttime = traj.time_from_start();
-    EXPECT_EQ(traj.end(), traj.sample(rclcpp::Time::now()));
-    auto allowed_delta = 10000ull;
+    EXPECT_EQ(traj.end(), traj.sample(rclcpp::Clock().now()));
+    auto allowed_delta = 10000ll;
     EXPECT_TRUE(traj.time_from_start().nanoseconds() - now.nanoseconds() < allowed_delta);
   }
 }
@@ -86,46 +88,46 @@ TEST_F(TestTrajectory, sample_trajectory) {
 
   auto traj = ros_controllers::Trajectory(full_msg);
 
-  auto sample_p1 = traj.sample(rclcpp::Time::now());
+  auto sample_p1 = traj.sample(rclcpp::Clock().now());
   ASSERT_NE(traj.end(), sample_p1);
   EXPECT_EQ(1.0f, sample_p1->positions[0]);
 
-  auto sample_p11 = traj.sample(rclcpp::Time::now());
+  auto sample_p11 = traj.sample(rclcpp::Clock().now());
   ASSERT_NE(traj.end(), sample_p11);
   EXPECT_EQ(1.0f, sample_p11->positions[0]);
 
   std::this_thread::sleep_for(1s);
 
-  auto sample_p2 = traj.sample(rclcpp::Time::now());
+  auto sample_p2 = traj.sample(rclcpp::Clock().now());
   ASSERT_NE(traj.end(), sample_p1);
   EXPECT_EQ(2.0f, sample_p2->positions[0]);
 
-  auto sample_p22 = traj.sample(rclcpp::Time::now());
+  auto sample_p22 = traj.sample(rclcpp::Clock().now());
   ASSERT_NE(traj.end(), sample_p22);
   EXPECT_EQ(2.0f, sample_p22->positions[0]);
 
   std::this_thread::sleep_for(1s);
 
-  auto sample_p3 = traj.sample(rclcpp::Time::now());
+  auto sample_p3 = traj.sample(rclcpp::Clock().now());
   ASSERT_NE(traj.end(), sample_p1);
   EXPECT_EQ(3.0f, sample_p3->positions[0]);
 
-  auto sample_p33 = traj.sample(rclcpp::Time::now());
+  auto sample_p33 = traj.sample(rclcpp::Clock().now());
   ASSERT_NE(traj.end(), sample_p33);
   EXPECT_EQ(3.0f, sample_p33->positions[0]);
 
   std::this_thread::sleep_for(1s);
 
-  auto sample_end = traj.sample(rclcpp::Time::now());
+  auto sample_end = traj.sample(rclcpp::Clock().now());
   EXPECT_EQ(traj.end(), sample_end);
 
-  auto sample_end_end = traj.sample(rclcpp::Time::now());
+  auto sample_end_end = traj.sample(rclcpp::Clock().now());
   EXPECT_EQ(traj.end(), sample_end_end);
 }
 
 TEST_F(TestTrajectory, future_sample_trajectory) {
   auto full_msg = std::make_shared<trajectory_msgs::msg::JointTrajectory>();
-  full_msg->header.stamp = rclcpp::Time::now();
+  full_msg->header.stamp = rclcpp::Clock().now();
   full_msg->header.stamp.sec += 2;  // extra padding
 
   trajectory_msgs::msg::JointTrajectoryPoint p1;
@@ -156,6 +158,6 @@ TEST_F(TestTrajectory, future_sample_trajectory) {
   auto traj = ros_controllers::Trajectory(full_msg);
 
   // sample for future point
-  auto sample_p0 = traj.sample(rclcpp::Time::now());
+  auto sample_p0 = traj.sample(rclcpp::Clock().now());
   ASSERT_EQ(traj.end(), sample_p0);
 }

ros_controllers/test/test_trajectory_controller.cpp

@@ -39,7 +39,7 @@
 using lifecycle_msgs::msg::State;
 
 void
-spin(rclcpp::executors::multi_threaded_executor::MultiThreadedExecutor * exe)
+spin(rclcpp::executors::MultiThreadedExecutor * exe)
 {
   exe->spin();
 }
@@ -99,18 +99,18 @@ class TestTrajectoryController : public ::testing::Test
     traj_msg_ptr->header.stamp.nanosec = 0;
     traj_msg_ptr->points.resize(points.size());
 
-    builtin_interfaces::msg::Time duration_msg;
+    builtin_interfaces::msg::Duration duration_msg;
     duration_msg.sec = time_from_start.sec;
     duration_msg.nanosec = time_from_start.nanosec;
-    rclcpp::Time duration(duration_msg);
-    rclcpp::Time duration_total(duration_msg);
+    rclcpp::Duration duration(duration_msg);
+    rclcpp::Duration duration_total(duration_msg);
 
     size_t index = 0;
     for (; index < points.size(); ++index) {
       traj_msg_ptr->points[index].time_from_start.sec =
-        static_cast<builtin_interfaces::msg::Time>(duration_total).sec;
+        duration_total.nanoseconds() / 1e9;
       traj_msg_ptr->points[index].time_from_start.nanosec =
-        static_cast<builtin_interfaces::msg::Time>(duration_total).nanosec;
+        duration_total.nanoseconds();
       traj_msg_ptr->points[index].positions.resize(3);
       traj_msg_ptr->points[index].positions[0] = points[index][0];
       traj_msg_ptr->points[index].positions[1] = points[index][1];
@@ -169,7 +169,7 @@ TEST_F(TestTrajectoryController, configuration) {
     FAIL();
   }
 
-  rclcpp::executors::multi_threaded_executor::MultiThreadedExecutor executor;
+  rclcpp::executors::MultiThreadedExecutor executor;
   executor.add_node(traj_controller->get_lifecycle_node()->get_node_base_interface());
   auto future_handle_ = std::async(std::launch::async, spin, &executor);
 
@@ -204,7 +204,7 @@ TEST_F(TestTrajectoryController, activation) {
   }
 
   auto traj_lifecycle_node = traj_controller->get_lifecycle_node();
-  rclcpp::executors::multi_threaded_executor::MultiThreadedExecutor executor;
+  rclcpp::executors::MultiThreadedExecutor executor;
   executor.add_node(traj_lifecycle_node->get_node_base_interface());
 
   auto state = traj_lifecycle_node->configure();
@@ -246,7 +246,7 @@ TEST_F(TestTrajectoryController, reactivation) {
   }
 
   auto traj_lifecycle_node = traj_controller->get_lifecycle_node();
-  rclcpp::executors::multi_threaded_executor::MultiThreadedExecutor executor;
+  rclcpp::executors::MultiThreadedExecutor executor;
   executor.add_node(traj_lifecycle_node->get_node_base_interface());
 
   auto state = traj_lifecycle_node->configure();
@@ -315,7 +315,7 @@ TEST_F(TestTrajectoryController, cleanup) {
   }
 
   auto traj_lifecycle_node = traj_controller->get_lifecycle_node();
-  rclcpp::executors::multi_threaded_executor::MultiThreadedExecutor executor;
+  rclcpp::executors::MultiThreadedExecutor executor;
   executor.add_node(traj_lifecycle_node->get_node_base_interface());
 
   auto state = traj_lifecycle_node->configure();
@@ -368,7 +368,6 @@ TEST_F(TestTrajectoryController, correct_initialization_with_config_file) {
   // must be related to STL containers and windows
   std::string file_path = config_file;
   auto ps = std::make_shared<controller_parameter_server::ParameterServer>();
-  ps->init();
   ps->load_parameters(file_path);
 
   auto traj_controller = std::make_shared<ros_controllers::JointTrajectoryController>();
@@ -377,15 +376,15 @@ TEST_F(TestTrajectoryController, correct_initialization_with_config_file) {
     FAIL();
   }
 
-  rclcpp::executors::multi_threaded_executor::MultiThreadedExecutor executor;
+  rclcpp::executors::MultiThreadedExecutor executor;
   executor.add_node(ps);
   auto traj_lifecycle_node = traj_controller->get_lifecycle_node();
   executor.add_node(traj_lifecycle_node->get_node_base_interface());
 
   auto future_handle = std::async(
     std::launch::async, [&executor]() -> void {
-    executor.spin();
-  });
+      executor.spin();
+    });
 
   auto state = traj_lifecycle_node->configure();
   ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id());