diff --git a/ros_controllers/include/ros_controllers/trajectory.hpp b/ros_controllers/include/ros_controllers/trajectory.hpp index 4ed4b3fdfd..764bb94ba2 100644 --- a/ros_controllers/include/ros_controllers/trajectory.hpp +++ b/ros_controllers/include/ros_controllers/trajectory.hpp @@ -28,9 +28,9 @@ namespace ros_controllers { using TrajectoryPointIter = - std::vector::iterator; + std::vector::iterator; using TrajectoryPointConstIter = - std::vector::const_iterator; + std::vector::const_iterator; class Trajectory { diff --git a/ros_controllers/src/joint_state_controller.cpp b/ros_controllers/src/joint_state_controller.cpp index 060e6d0f18..a7fbcc374b 100644 --- a/ros_controllers/src/joint_state_controller.cpp +++ b/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) diff --git a/ros_controllers/src/joint_trajectory_controller.cpp b/ros_controllers/src/joint_trajectory_controller.cpp index c9f024140b..cb694395bc 100644 --- a/ros_controllers/src/joint_trajectory_controller.cpp +++ b/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 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) diff --git a/ros_controllers/src/trajectory.cpp b/ros_controllers/src/trajectory.cpp index dda5cab60a..b125c8ad72 100644 --- a/ros_controllers/src/trajectory.cpp +++ b/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 joint_trajectory) : trajectory_msg_(joint_trajectory), trajectory_start_time_(time_is_zero(joint_trajectory->header.stamp) ? - rclcpp::Time::now() : + rclcpp::Clock().now() : static_cast(joint_trajectory->header.stamp)) {} @@ -44,7 +46,7 @@ Trajectory::update(std::shared_ptr joint_ { trajectory_msg_ = joint_trajectory; trajectory_start_time_ = (time_is_zero(joint_trajectory->header.stamp) ? - rclcpp::Time::now() : + rclcpp::Clock().now() : static_cast(joint_trajectory->header.stamp)); } diff --git a/ros_controllers/test/test_trajectory.cpp b/ros_controllers/test/test_trajectory.cpp index 237d9682ba..99206220e6 100644 --- a/ros_controllers/test/test_trajectory.cpp +++ b/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(); 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(); - 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); } diff --git a/ros_controllers/test/test_trajectory_controller.cpp b/ros_controllers/test/test_trajectory_controller.cpp index 0270df57e3..57e6c78b9d 100644 --- a/ros_controllers/test/test_trajectory_controller.cpp +++ b/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(duration_total).sec; + duration_total.nanoseconds() / 1e9; traj_msg_ptr->points[index].time_from_start.nanosec = - static_cast(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(); - ps->init(); ps->load_parameters(file_path); auto traj_controller = std::make_shared(); @@ -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());