import ros_controllers

ros_controllers/CMakeLists.txt

@@ -0,0 +1,109 @@
+cmake_minimum_required(VERSION 3.5)
+project(ros_controllers)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(controller_interface REQUIRED)
+find_package(controller_manager REQUIRED)
+find_package(controller_parameter_server REQUIRED)
+find_package(hardware_interface REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(trajectory_msgs REQUIRED)
+
+add_library(
+  default_controllers
+  SHARED
+  src/joint_trajectory_controller.cpp
+  src/joint_state_controller.cpp
+  src/trajectory.cpp
+)
+target_include_directories(default_controllers PRIVATE include)
+ament_target_dependencies(
+  default_controllers
+  "builtin_interfaces"
+  "class_loader"
+  "controller_interface"
+  "controller_manager"
+  "controller_parameter_server"
+  "hardware_interface"
+  "rclcpp"
+  "rclcpp_lifecycle"
+  "rcutils"
+  "sensor_msgs"
+  "trajectory_msgs"
+)
+controller_manager_register_controller(
+  default_controllers
+  "ros_controllers::JointStateController"
+  "ros_controllers::JointTrajectoryController"
+)
+# Causes the visibility macros to use dllexport rather than dllimport,
+# which is appropriate when building the dll but not consuming it.
+target_compile_definitions(default_controllers PRIVATE "ROS_CONTROLLERS_BUILDING_DLL")
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+install(
+  TARGETS
+  default_controllers
+  RUNTIME DESTINATION bin
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+
+  find_package(controller_parameter_server)
+
+  ament_add_gtest(test_trajectory test/test_trajectory.cpp)
+  target_include_directories(test_trajectory PRIVATE include)
+  target_link_libraries(test_trajectory default_controllers)
+
+  ament_add_gtest(
+    test_trajectory_controller test/test_trajectory_controller.cpp
+    ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_joint_trajectory_controller.yaml)
+  target_include_directories(test_trajectory_controller PRIVATE include)
+  target_link_libraries(
+    test_trajectory_controller
+    default_controllers
+  )
+  ament_target_dependencies(
+    test_trajectory_controller
+    "controller_interface"
+    "controller_parameter_server"
+    "hardware_interface"
+    "lifecycle_msgs"
+    "rclcpp"
+    "rcutils"
+    "test_robot_hardware"
+    "trajectory_msgs"
+  )
+endif()
+
+ament_export_dependencies(
+  controller_interface
+  rclcpp_lifecycle
+  sensor_msgs
+  trajectory_msgs
+)
+ament_export_include_directories(
+  include
+)
+ament_export_libraries(
+  default_controllers
+)
+ament_package()

ros_controllers/include/ros_controllers/joint_state_controller.hpp

@@ -0,0 +1,58 @@
+// Copyright 2017 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef ROS_CONTROLLERS__JOINT_STATE_CONTROLLER_HPP_
+#define ROS_CONTROLLERS__JOINT_STATE_CONTROLLER_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "controller_interface/controller_interface.hpp"
+
+#include "hardware_interface/robot_hardware.hpp"
+
+#include "rclcpp_lifecycle/state.hpp"
+
+#include "ros_controllers/visibility_control.h"
+
+#include "sensor_msgs/msg/joint_state.hpp"
+
+namespace ros_controllers
+{
+
+class JointStateController : public controller_interface::ControllerInterface
+{
+public:
+  ROS_CONTROLLERS_PUBLIC
+  JointStateController();
+
+  ROS_CONTROLLERS_PUBLIC
+  controller_interface::controller_interface_ret_t
+  update() override;
+
+  ROS_CONTROLLERS_PUBLIC
+  rcl_lifecycle_transition_key_t
+  on_configure(const rclcpp_lifecycle::State & previous_state) override;
+
+private:
+  std::vector<const hardware_interface::JointStateHandle *> registered_joint_handles_;
+  std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<sensor_msgs::msg::JointState>>
+  joint_state_publisher_;
+  std::shared_ptr<sensor_msgs::msg::JointState> joint_state_msg_;
+};
+
+}  // namespace ros_controllers
+
+#endif  // ROS_CONTROLLERS__JOINT_STATE_CONTROLLER_HPP_

ros_controllers/include/ros_controllers/joint_trajectory_controller.hpp

@@ -0,0 +1,105 @@
+// Copyright 2017 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef ROS_CONTROLLERS__JOINT_TRAJECTORY_CONTROLLER_HPP_
+#define ROS_CONTROLLERS__JOINT_TRAJECTORY_CONTROLLER_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "controller_interface/controller_interface.hpp"
+
+#include "hardware_interface/operation_mode_handle.hpp"
+#include "hardware_interface/robot_hardware.hpp"
+
+#include "rclcpp_lifecycle/state.hpp"
+
+#include "ros_controllers/trajectory.hpp"
+#include "ros_controllers/visibility_control.h"
+
+#include "trajectory_msgs/msg/joint_trajectory.hpp"
+#include "trajectory_msgs/msg/joint_trajectory_point.hpp"
+
+namespace ros_controllers
+{
+
+class JointTrajectoryController : public controller_interface::ControllerInterface
+{
+public:
+  ROS_CONTROLLERS_PUBLIC
+  JointTrajectoryController();
+
+  ROS_CONTROLLERS_PUBLIC
+  JointTrajectoryController(
+    const std::vector<std::string> & joint_names,
+    const std::vector<std::string> & write_op_names);
+
+  ROS_CONTROLLERS_PUBLIC
+  rcl_lifecycle_transition_key_t
+  on_configure(const rclcpp_lifecycle::State & previous_state) override;
+
+  ROS_CONTROLLERS_PUBLIC
+  rcl_lifecycle_transition_key_t
+  on_activate(const rclcpp_lifecycle::State & previous_state) override;
+
+  ROS_CONTROLLERS_PUBLIC
+  rcl_lifecycle_transition_key_t
+  on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
+
+  ROS_CONTROLLERS_PUBLIC
+  rcl_lifecycle_transition_key_t
+  on_cleanup(const rclcpp_lifecycle::State & previous_state) override;
+
+  ROS_CONTROLLERS_PUBLIC
+  rcl_lifecycle_transition_key_t
+  on_error(const rclcpp_lifecycle::State & previous_state) override;
+
+  ROS_CONTROLLERS_PUBLIC
+  rcl_lifecycle_transition_key_t
+  on_shutdown(const rclcpp_lifecycle::State & previous_state) override;
+
+  ROS_CONTROLLERS_PUBLIC
+  controller_interface::controller_interface_ret_t
+  update() override;
+
+private:
+  std::vector<std::string> joint_names_;
+  std::vector<std::string> write_op_names_;
+
+  std::vector<hardware_interface::JointCommandHandle *> registered_joint_cmd_handles_;
+  std::vector<const hardware_interface::JointStateHandle *> registered_joint_state_handles_;
+  std::vector<hardware_interface::OperationModeHandle *> registered_operation_mode_handles_;
+
+  // TODO(karsten1987): eventually activate and deactive subscriber directly when its supported
+  bool subscriber_is_active_ = false;
+  rclcpp::subscription::Subscription<trajectory_msgs::msg::JointTrajectory>::SharedPtr
+    joint_command_subscriber_ = nullptr;
+
+  TrajectoryPointConstIter prev_traj_point_ptr_;
+  std::shared_ptr<Trajectory> * traj_point_active_ptr_ = nullptr;
+  std::shared_ptr<Trajectory> traj_external_point_ptr_ = nullptr;
+  std::shared_ptr<Trajectory> traj_home_point_ptr_ = nullptr;
+  std::shared_ptr<trajectory_msgs::msg::JointTrajectory> traj_msg_home_ptr_ = nullptr;
+
+  bool is_halted = false;
+
+  bool reset();
+  void set_op_mode(const hardware_interface::OperationMode & mode);
+  void halt();
+};
+
+}  // namespace ros_controllers
+
+#endif  // ROS_CONTROLLERS__JOINT_TRAJECTORY_CONTROLLER_HPP_

ros_controllers/include/ros_controllers/trajectory.hpp

@@ -0,0 +1,87 @@
+// Copyright 2017 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef ROS_CONTROLLERS__TRAJECTORY_HPP_
+#define ROS_CONTROLLERS__TRAJECTORY_HPP_
+
+#include <memory>
+#include <vector>
+
+#include "rclcpp/time.hpp"
+
+#include "ros_controllers/visibility_control.h"
+
+#include "trajectory_msgs/msg/joint_trajectory.hpp"
+
+namespace ros_controllers
+{
+
+using TrajectoryPointIter =
+    std::vector<trajectory_msgs::msg::JointTrajectoryPoint>::iterator;
+using TrajectoryPointConstIter =
+    std::vector<trajectory_msgs::msg::JointTrajectoryPoint>::const_iterator;
+
+class Trajectory
+{
+public:
+  ROS_CONTROLLERS_PUBLIC
+  Trajectory();
+
+  ROS_CONTROLLERS_PUBLIC
+  explicit Trajectory(std::shared_ptr<trajectory_msgs::msg::JointTrajectory> joint_trajectory);
+
+  ROS_CONTROLLERS_PUBLIC
+  void
+  update(std::shared_ptr<trajectory_msgs::msg::JointTrajectory> joint_trajectory);
+
+  /// Find the next valid point from the containing trajectory msg.
+  /**
+  * Within each msg, points with time_from_start less or equal than current time will be skipped.
+  * The first point with time_from_start greater than current time shall be a valid point,
+  * Return end iterator if start time of desired trajectory msg is in the future
+  * Else within each msg, valid point is the first point in the the msg with expected arrival time
+  * in the future.
+  * Arrival time is time_from_start of point + start time of msg
+  * If an empty trajectory message is given, sample will return Trajectory::end()
+  * If no valid point is found for the specified sample time, Trajectory::end() will be returned.
+  */
+  ROS_CONTROLLERS_PUBLIC
+  TrajectoryPointConstIter
+  sample(const rclcpp::Time & sample_time);
+
+  ROS_CONTROLLERS_PUBLIC
+  TrajectoryPointConstIter
+  begin() const;
+
+  ROS_CONTROLLERS_PUBLIC
+  TrajectoryPointConstIter
+  end() const;
+
+  ROS_CONTROLLERS_PUBLIC
+  rclcpp::Time
+  time_from_start() const;
+
+  ROS_CONTROLLERS_PUBLIC
+  bool
+  is_empty() const;
+
+private:
+  std::shared_ptr<trajectory_msgs::msg::JointTrajectory> trajectory_msg_;
+
+  rclcpp::Time trajectory_start_time_;
+};
+
+}  // namespace ros_controllers
+
+#endif  // ROS_CONTROLLERS__TRAJECTORY_HPP_