Refactoring structure of example_7

example_7/CMakeLists.txt

@@ -0,0 +1,109 @@
+cmake_minimum_required(VERSION 3.16)
+project(ros2_control_demo_example_7 LANGUAGES CXX)
+
+if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
+  add_compile_options(-Wall -Wextra)
+endif()
+
+# find dependencies
+set(HW_IF_INCLUDE_DEPENDS
+  pluginlib
+  rcpputils
+  hardware_interface
+)
+set(REF_GEN_INCLUDE_DEPENDS
+  kdl_parser
+  rclcpp
+  trajectory_msgs
+)
+set(CONTROLLER_INCLUDE_DEPENDS
+  pluginlib
+  rcpputils
+  controller_interface
+  realtime_tools
+  trajectory_msgs
+)
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+foreach(Dependency IN ITEMS ${HW_IF_INCLUDE_DEPENDS})
+  find_package(${Dependency} REQUIRED)
+endforeach()
+foreach(Dependency IN ITEMS ${REF_GEN_INCLUDE_DEPENDS})
+  find_package(${Dependency} REQUIRED)
+endforeach()
+foreach(Dependency IN ITEMS ${CONTROLLER_INCLUDE_DEPENDS})
+  find_package(${Dependency} REQUIRED)
+endforeach()
+
+
+## COMPILE
+add_executable(send_trajectory reference_generator/send_trajectory.cpp)
+
+ament_target_dependencies(
+  send_trajectory PUBLIC
+  ${REF_GEN_INCLUDE_DEPENDS}
+)
+
+add_library(
+  ros2_control_demo_example_7
+  SHARED
+  hardware/r6bot_hardware.cpp
+  controller/r6bot_controller.cpp
+)
+
+target_compile_features(ros2_control_demo_example_7 PUBLIC cxx_std_17)
+target_include_directories(ros2_control_demo_example_7 PUBLIC
+$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/hardware/include>
+$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/controller/include>
+$<INSTALL_INTERFACE:include/ros2_control_demo_example_7>
+)
+ament_target_dependencies(
+  ros2_control_demo_example_7 PUBLIC
+  ${HW_IF_INCLUDE_DEPENDS}
+  ${CONTROLLER_INCLUDE_DEPENDS}
+)
+
+# Causes the visibility macros to use dllexport rather than dllimport,
+# which is appropriate when building the dll but not consuming it.
+target_compile_definitions(${PROJECT_NAME} PRIVATE "ROS2_CONTROL_DEMO_EXAMPLE_7_BUILDING_DLL")
+
+# Export hardware plugins
+pluginlib_export_plugin_description_file(hardware_interface ros2_control_demo_example_7.xml)
+# Export controller plugins
+pluginlib_export_plugin_description_file(controller_interface ros2_control_demo_example_7.xml)
+
+# INSTALL
+install(
+  DIRECTORY hardware/include/
+  DESTINATION include/ros2_control_demo_example_7
+)
+install(
+  DIRECTORY description/launch description/ros2_control description/urdf description/rviz description/srdf  description/meshes
+  DESTINATION share/ros2_control_demo_example_7
+)
+install(
+  DIRECTORY bringup/launch bringup/config
+  DESTINATION share/ros2_control_demo_example_7
+)
+
+install(
+    TARGETS send_trajectory
+    RUNTIME DESTINATION lib/ros2_control_demo_example_7
+)
+
+install(TARGETS ros2_control_demo_example_7
+  EXPORT export_ros2_control_demo_example_7
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+endif()
+
+## EXPORTS
+ament_export_targets(export_ros2_control_demo_example_7 HAS_LIBRARY_TARGET)
+ament_export_dependencies(${HW_IF_INCLUDE_DEPENDS} ${REF_GEN_INCLUDE_DEPENDS} ${CONTROLLER_INCLUDE_DEPENDS})
+ament_package()

example_7/README.md

@@ -0,0 +1,12 @@
+# ros2_control_demo_example_7
+
+A full tutorial for a 6 DOF robot for intermediate ROS 2 users.
+
+It consists of the following:
+* bringup: launch files and ros2_controller configuration
+* controller: a controller for the 6-DOF robot
+* description: the 6-DOF robot description
+* hardware: ros2_control hardware interface
+* reference_generator: A KDL-based reference generator for a fixed trajectory
+
+Find the documentation in [doc/userdoc.rst](doc/userdoc.rst) or on [control.ros.org](https://control.ros.org/master/doc/ros2_control_demos/example_7/doc/userdoc.html).

r6bot/r6bot_controller/config/r6bot_controller.yaml → example_7/bringup/config/r6bot_controller.yaml

@@ -6,7 +6,7 @@ controller_manager:
       type: joint_state_broadcaster/JointStateBroadcaster
 
     r6bot_controller:
-      type: r6bot_controller/RobotController
+      type: ros2_control_demo_example_7/RobotController
 
 
 r6bot_controller:

r6bot/r6bot_controller/launch/r6bot_controller.launch.py → example_7/bringup/launch/r6bot_controller.launch.py

@@ -29,7 +29,7 @@ def generate_launch_description():
             " ",
             PathJoinSubstitution(
                 [
-                    FindPackageShare("r6bot_description"),
+                    FindPackageShare("ros2_control_demo_example_7"),
                     "urdf",
                     "r6bot.urdf.xacro",
                 ]
@@ -40,13 +40,13 @@ def generate_launch_description():
 
     robot_controllers = PathJoinSubstitution(
         [
-            FindPackageShare("r6bot_controller"),
+            FindPackageShare("ros2_control_demo_example_7"),
             "config",
             "r6bot_controller.yaml",
         ]
     )
     rviz_config_file = PathJoinSubstitution(
-        [FindPackageShare("r6bot_description"), "rviz", "view_robot.rviz"]
+        [FindPackageShare("ros2_control_demo_example_7"), "rviz", "view_robot.rviz"]
     )
 
     control_node = Node(

r6bot/reference_generator/launch/send_trajectory.launch.py → example_7/bringup/launch/send_trajectory.launch.py

@@ -26,7 +26,7 @@ def generate_launch_description():
             " ",
             PathJoinSubstitution(
                 [
-                    FindPackageShare("r6bot_description"),
+                    FindPackageShare("ros2_control_demo_example_7"),
                     "urdf",
                     "r6bot.urdf.xacro",
                 ]
@@ -36,7 +36,7 @@ def generate_launch_description():
     robot_description = {"robot_description": robot_description_content}
 
     send_trajectory_node = Node(
-        package="reference_generator",
+        package="ros2_control_demo_example_7",
         executable="send_trajectory",
         name="send_trajectory_node",
         parameters=[robot_description],

r6bot/r6bot_controller/include/r6bot_controller/r6bot_controller.hpp → example_7/controller/include/ros2_control_demo_example_7/r6bot_controller.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef R6BOT_CONTROLLER__R6BOT_CONTROLLER_HPP_
-#define R6BOT_CONTROLLER__R6BOT_CONTROLLER_HPP_
+#ifndef ROS2_CONTROL_DEMO_EXAMPLE_7__R6BOT_CONTROLLER_HPP_
+#define ROS2_CONTROL_DEMO_EXAMPLE_7__R6BOT_CONTROLLER_HPP_
 
 #include <chrono>
 #include <memory>
@@ -37,7 +37,7 @@
 #include "trajectory_msgs/msg/joint_trajectory.hpp"
 #include "trajectory_msgs/msg/joint_trajectory_point.hpp"
 
-namespace r6bot_controller
+namespace ros2_control_demo_example_7
 {
 class RobotController : public controller_interface::ControllerInterface
 {
@@ -117,6 +117,6 @@ class RobotController : public controller_interface::ControllerInterface
       {"velocity", &joint_velocity_state_interface_}};
 };
 
-}  // namespace r6bot_controller
+}  // namespace ros2_control_demo_example_7
 
-#endif  // R6BOT_CONTROLLER__R6BOT_CONTROLLER_HPP_
+#endif  // ROS2_CONTROL_DEMO_EXAMPLE_7__R6BOT_CONTROLLER_HPP_

r6bot/r6bot_controller/src/r6bot_controller.cpp → example_7/controller/r6bot_controller.cpp

@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "r6bot_controller/r6bot_controller.hpp"
+#include "ros2_control_demo_example_7/r6bot_controller.hpp"
 
 #include <stddef.h>
 #include <algorithm>
@@ -27,7 +27,7 @@
 
 using config_type = controller_interface::interface_configuration_type;
 
-namespace r6bot_controller
+namespace ros2_control_demo_example_7
 {
 RobotController::RobotController() : controller_interface::ControllerInterface() {}
 
@@ -123,11 +123,11 @@ void interpolate_point(
   const trajectory_msgs::msg::JointTrajectoryPoint & point_2,
   trajectory_msgs::msg::JointTrajectoryPoint & point_interp, double delta)
 {
-  for (int i = 0; i < point_1.positions.size(); i++)
+  for (size_t i = 0; i < point_1.positions.size(); i++)
   {
     point_interp.positions[i] = delta * point_2.positions[i] + (1.0 - delta) * point_2.positions[i];
   }
-  for (int i = 0; i < point_1.positions.size(); i++)
+  for (size_t i = 0; i < point_1.positions.size(); i++)
   {
     point_interp.velocities[i] =
       delta * point_2.velocities[i] + (1.0 - delta) * point_2.velocities[i];
@@ -149,7 +149,7 @@ void interpolate_trajectory_point(
 }
 
 controller_interface::return_type RobotController::update(
-  const rclcpp::Time & time, const rclcpp::Duration & period)
+  const rclcpp::Time & time, const rclcpp::Duration & /*period*/)
 {
   if (new_msg_)
   {
@@ -161,11 +161,11 @@ controller_interface::return_type RobotController::update(
   if (trajectory_msg_ != nullptr)
   {
     interpolate_trajectory_point(*trajectory_msg_, time - start_time_, point_interp_);
-    for (int i = 0; i < joint_position_command_interface_.size(); i++)
+    for (size_t i = 0; i < joint_position_command_interface_.size(); i++)
     {
       joint_position_command_interface_[i].get().set_value(point_interp_.positions[i]);
     }
-    for (int i = 0; i < joint_velocity_command_interface_.size(); i++)
+    for (size_t i = 0; i < joint_velocity_command_interface_.size(); i++)
     {
       joint_velocity_command_interface_[i].get().set_value(point_interp_.velocities[i]);
     }
@@ -196,8 +196,9 @@ controller_interface::CallbackReturn RobotController::on_shutdown(const rclcpp_l
   return CallbackReturn::SUCCESS;
 }
 
-}  // namespace r6bot_controller
+}  // namespace ros2_control_demo_example_7
 
 #include "pluginlib/class_list_macros.hpp"
 
-PLUGINLIB_EXPORT_CLASS(r6bot_controller::RobotController, controller_interface::ControllerInterface)
+PLUGINLIB_EXPORT_CLASS(
+  ros2_control_demo_example_7::RobotController, controller_interface::ControllerInterface)

r6bot/r6bot_description/launch/view_r6bot.launch.py → example_7/description/launch/view_r6bot.launch.py

@@ -13,20 +13,35 @@
 # limitations under the License.
 
 from launch import LaunchDescription
-from launch.substitutions import Command, FindExecutable, PathJoinSubstitution
+from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition
+from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
 
 
 def generate_launch_description():
+    # Declare arguments
+    declared_arguments = []
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "gui",
+            default_value="true",
+            description="Start Rviz2 and Joint State Publisher gui automatically \
+        with this launch file.",
+        )
+    )
+    # Initialize Arguments
+    gui = LaunchConfiguration("gui")
+
     # Get URDF via xacro
     robot_description_content = Command(
         [
             PathJoinSubstitution([FindExecutable(name="xacro")]),
             " ",
             PathJoinSubstitution(
                 [
-                    FindPackageShare("r6bot_description"),
+                    FindPackageShare("ros2_control_demo_example_7"),
                     "urdf",
                     "r6bot.urdf.xacro",
                 ]
@@ -36,12 +51,13 @@ def generate_launch_description():
     robot_description = {"robot_description": robot_description_content}
 
     rviz_config_file = PathJoinSubstitution(
-        [FindPackageShare("r6bot_description"), "rviz", "view_robot.rviz"]
+        [FindPackageShare("ros2_control_demo_example_7"), "rviz", "view_robot.rviz"]
     )
 
     joint_state_publisher_node = Node(
-        package="joint_state_publisher",
-        executable="joint_state_publisher",
+        package="joint_state_publisher_gui",
+        executable="joint_state_publisher_gui",
+        condition=IfCondition(gui),
     )
 
     robot_state_publisher_node = Node(
@@ -56,6 +72,7 @@ def generate_launch_description():
         name="rviz2",
         output="log",
         arguments=["-d", rviz_config_file],
+        condition=IfCondition(gui),
     )
     #
     nodes_to_start = [
@@ -64,4 +81,4 @@ def generate_launch_description():
         rviz_node,
     ]
 
-    return LaunchDescription(nodes_to_start)
+    return LaunchDescription(declared_arguments + nodes_to_start)