diff --git a/ros2_control_demo_communication_headless/CMakeLists.txt b/ros2_control_demo_communication_headless/CMakeLists.txt index 5deec2a05..03e2d23ee 100644 --- a/ros2_control_demo_communication_headless/CMakeLists.txt +++ b/ros2_control_demo_communication_headless/CMakeLists.txt @@ -13,6 +13,30 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(ros2_control_core REQUIRED) + +## COMPILE +add_library(${PROJECT_NAME} SHARED + src/demo_robot_headless.cpp +) + +target_include_directories(${PROJECT_NAME} + PRIVATE include +) + +ament_target_dependencies(${PROJECT_NAME} + pluginlib + rclcpp + ros2_control_core +) + +pluginlib_export_plugin_description_file(ros2_control_core ros2_control_demo_communication_headless.xml) + +# INSTALL +install(TARGETS ${PROJECT_NAME} + DESTINATION lib +) install(DIRECTORY include/ DESTINATION include) @@ -26,6 +50,14 @@ if(BUILD_TESTING) endif() +## EXPORTS +ament_export_dependencies( + pluginlib + rclcpp + ros2_control_core +) ament_export_libraries(${PROJECT_NAME}) -ament_export_include_directories(include) +ament_export_include_directories( + include +) ament_package() diff --git a/ros2_control_demo_communication_headless/include/ros2_control_demo_communication_headless/demo_robot_headless.hpp b/ros2_control_demo_communication_headless/include/ros2_control_demo_communication_headless/demo_robot_headless.hpp new file mode 100644 index 000000000..bdb5298bd --- /dev/null +++ b/ros2_control_demo_communication_headless/include/ros2_control_demo_communication_headless/demo_robot_headless.hpp @@ -0,0 +1,46 @@ +// Copyright 2020 ROS2-Control Development Team +// +// 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 ROS2_CONTROL_DEMO_COMMUNICATION__DEMO_ROBOT_HEADLESS_HPP_ +#define ROS2_CONTROL_DEMO_COMMUNICATION__DEMO_ROBOT_HEADLESS_HPP_ + +#include "rclcpp/rate.hpp" +#include "rclcpp/rclcpp.hpp" +#include "ros2_control_core/communication_interface/hardware_communication_interface.hpp" +#include "ros2_control_core/ros2_control_types.h" +#include "ros2_control_demo_communication_headless/visibility_control.h" + + +namespace ros2_control_demo_communication_headless +{ +class DemoRobotHeadless : public ros2_control_core_communication_interface::HardwareCommunicationInterface +{ +public: + RCLCPP_SHARED_PTR_DEFINITIONS(DemoRobotHeadless); + + ROS2_CONTROL_DEMO_COMMUNICATION_HEADLESS_PUBLIC DemoRobotHeadless() = default; + + ROS2_CONTROL_DEMO_COMMUNICATION_HEADLESS_PUBLIC virtual ~DemoRobotHeadless() = default; + + ROS2_CONTROL_DEMO_COMMUNICATION_HEADLESS_PUBLIC ros2_control_types::return_type configure(const std::string parameters_path, const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface, const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameters_interface, const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr services_interface); + + ROS2_CONTROL_DEMO_COMMUNICATION_HEADLESS_PUBLIC ros2_control_types::return_type init(); + +protected: +}; + +} // namespace ros2_control_core_components + +#endif // ROS2_CONTROL_DEMO_COMMUNICATION__DEMO_ROBOT_HEADLESS_HPP_ diff --git a/ros2_control_demo_communication_headless/ros2_control_demo_communication_headless.xml b/ros2_control_demo_communication_headless/ros2_control_demo_communication_headless.xml new file mode 100644 index 000000000..317df28a7 --- /dev/null +++ b/ros2_control_demo_communication_headless/ros2_control_demo_communication_headless.xml @@ -0,0 +1,7 @@ + + + + The ROS2 Control communication interface for headless demo robot. + + + diff --git a/ros2_control_demo_communication_headless/src/demo_robot_headless.cpp b/ros2_control_demo_communication_headless/src/demo_robot_headless.cpp new file mode 100644 index 000000000..1fad016e1 --- /dev/null +++ b/ros2_control_demo_communication_headless/src/demo_robot_headless.cpp @@ -0,0 +1,47 @@ +// Copyright 2020 ROS2-Control Development Team +// +// 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. + +#include "ros2_control_demo_communication_headless/demo_robot_headless.hpp" + +using namespace ros2_control_demo_communication_headless; + +ros2_control_types::return_type DemoRobotHeadless::configure(const std::string parameters_path, const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface, const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameters_interface, const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr services_interface) +{ + ros2_control_types::return_type ret; + ret = HardwareCommunicationInterface::configure(parameters_path, logging_interface, parameters_interface, services_interface); + + RCLCPP_INFO(logging_interface_->get_logger(), "Configuring DemoRobotHeadless communication ..."); + + return ret; +} + +ros2_control_types::return_type DemoRobotHeadless::init() +{ + RCLCPP_INFO(logging_interface_->get_logger(), "Initalizing DemoRobotHeadless communication ...please wait..."); + + for (int i=0; i < 2; i++) + { + rclcpp::sleep_for(std::chrono::milliseconds(1*1000/2)); + RCLCPP_INFO(logging_interface_->get_logger(), "%f seconds left...", 1-(i/2.0)); + } + + RCLCPP_INFO(logging_interface_->get_logger(), "DemoRobotHeadless communication sucessfully initalized!"); + + return ros2_control_types::ROS2C_RETURN_OK; +} + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + ros2_control_demo_communication_headless::DemoRobotHeadless, ros2_control_core_communication_interface::HardwareCommunicationInterface) diff --git a/ros2_control_demo_hardware/CMakeLists.txt b/ros2_control_demo_hardware/CMakeLists.txt index 8bd9c45aa..896a077c5 100644 --- a/ros2_control_demo_hardware/CMakeLists.txt +++ b/ros2_control_demo_hardware/CMakeLists.txt @@ -13,6 +13,31 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(ros2_control_core REQUIRED) + +## COMPILE +add_library(${PROJECT_NAME} SHARED + src/demo_robot_hardware_headless.cpp + src/demo_robot_hardware_minimal.cpp +) + +target_include_directories(${PROJECT_NAME} + PRIVATE include +) + +ament_target_dependencies(${PROJECT_NAME} + pluginlib + rclcpp + ros2_control_core +) + +pluginlib_export_plugin_description_file(ros2_control_hardware ros2_control_demo_hardware_headless.xml) + +# INSTALL +install(TARGETS ${PROJECT_NAME} + DESTINATION lib +) install(DIRECTORY include/ DESTINATION include) @@ -26,6 +51,14 @@ if(BUILD_TESTING) endif() +## EXPORTS +ament_export_dependencies( + pluginlib + rclcpp + ros2_control_core +) ament_export_libraries(${PROJECT_NAME}) -ament_export_include_directories(include) +ament_export_include_directories( + include +) ament_package() diff --git a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/demo_robot_hardware_headless.hpp b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/demo_robot_hardware_headless.hpp new file mode 100644 index 000000000..93da4d7c8 --- /dev/null +++ b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/demo_robot_hardware_headless.hpp @@ -0,0 +1,41 @@ +// Copyright 2020 ROS2-Control Development Team +// +// 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 ROS2_CONTROL_DEMO_HARDWARE__DEMO_ROBOT_HARDWARE_HEADLESS_HPP_ +#define ROS2_CONTROL_DEMO_HARDWARE__DEMO_ROBOT_HARDWARE_HEADLESS_HPP_ + +#include "rclcpp/rate.hpp" +#include "rclcpp/rclcpp.hpp" +#include "ros2_control_core/hardware/robot_hardware.hpp" +#include "ros2_control_demo_hardware/visibility_control.h" + + +namespace ros2_control_demo_hardware_headless +{ +class DemoRobotHardwareHeadless : public ros2_control_core_hardware::RobotHardware +{ +public: + ROS2_CONTROL_DEMO_HARDWARE_PUBLIC DemoRobotHardwareHeadless() = default; + + ROS2_CONTROL_DEMO_HARDWARE_PUBLIC ~DemoRobotHardwareHeadless() = default; + + ROS2_CONTROL_DEMO_HARDWARE_PUBLIC ros2_control_types::return_type init_hardware(); + +protected: +}; + +} // namespace ros2_control_core_components + +#endif // ROS2_CONTROL_DEMO_HARDWARE__DEMO_ROBOT_HARDWARE_HEADLESS_HPP_ diff --git a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/demo_robot_hardware_minimal.hpp b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/demo_robot_hardware_minimal.hpp new file mode 100644 index 000000000..f03b8eb0a --- /dev/null +++ b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/demo_robot_hardware_minimal.hpp @@ -0,0 +1,42 @@ +// Copyright 2020 ROS2-Control Development Team +// +// 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 ROS2_CONTROL_DEMO_HARDWARE__DEMO_ROBOT_HARDWARE_MINIMAL_HPP_ +#define ROS2_CONTROL_DEMO_HARDWARE__DEMO_ROBOT_HARDWARE_MINIMAL_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include "ros2_control_core/hardware/robot_hardware.hpp" +#include "ros2_control_demo_hardware/visibility_control.h" + + +namespace ros2_control_demo_hardware_minimal +{ +class DemoRobotHardwareMinimal : public ros2_control_core_hardware::RobotHardware +{ +public: + ROS2_CONTROL_DEMO_HARDWARE_PUBLIC DemoRobotHardwareMinimal() = default; + + ROS2_CONTROL_DEMO_HARDWARE_PUBLIC ~DemoRobotHardwareMinimal() = default; + + ROS2_CONTROL_DEMO_HARDWARE_PUBLIC ros2_control_types::return_type init_hardware(); + + ROS2_CONTROL_DEMO_HARDWARE_PUBLIC ros2_control_types::return_type write(); + +protected: +}; + +} // namespace ros2_control_core_components + +#endif // ROS2_CONTROL_DEMO_HARDWARE__DEMO_ROBOT_HARDWARE_MINIMAL_HPP_ diff --git a/ros2_control_demo_hardware/ros2_control_demo_hardware_headless.xml b/ros2_control_demo_hardware/ros2_control_demo_hardware_headless.xml new file mode 100644 index 000000000..7c89cafaa --- /dev/null +++ b/ros2_control_demo_hardware/ros2_control_demo_hardware_headless.xml @@ -0,0 +1,12 @@ + + + + The ROS2 Control robot protocol for headless demo robot. + + + + + The ROS2 Control minimal robot protocol. This is example for start porting the robot from ROS 1. + + + diff --git a/ros2_control_demo_hardware/src/demo_robot_hardware_headless.cpp b/ros2_control_demo_hardware/src/demo_robot_hardware_headless.cpp new file mode 100644 index 000000000..acf185cba --- /dev/null +++ b/ros2_control_demo_hardware/src/demo_robot_hardware_headless.cpp @@ -0,0 +1,38 @@ +// Copyright 2020 ROS2-Control Development Team +// +// 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. + + +#include "ros2_control_demo_hardware/demo_robot_hardware_headless.hpp" + +using namespace ros2_control_demo_hardware_headless; + +ros2_control_types::return_type DemoRobotHardwareHeadless::init_hardware() +{ + RCLCPP_INFO(logging_interface_->get_logger(), "Initalizing DemoRobotHardwareHeadless ...please wait..."); + + for (int i=0; i < 3; i++) + { + rclcpp::sleep_for(std::chrono::seconds(1)); + RCLCPP_INFO(logging_interface_->get_logger(), "%d seconds left...", 3-i); + } + + RCLCPP_INFO(logging_interface_->get_logger(), "DemoRobotHardwareHeadless sucessfully initalized!"); + + return ros2_control_types::ROS2C_RETURN_OK; +} + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + ros2_control_demo_hardware_headless::DemoRobotHardwareHeadless, ros2_control_core_hardware::RobotHardware) diff --git a/ros2_control_demo_hardware/src/demo_robot_hardware_minimal.cpp b/ros2_control_demo_hardware/src/demo_robot_hardware_minimal.cpp new file mode 100644 index 000000000..6ce065758 --- /dev/null +++ b/ros2_control_demo_hardware/src/demo_robot_hardware_minimal.cpp @@ -0,0 +1,38 @@ +// Copyright 2020 ROS2-Control Development Team +// +// 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. + + +#include "ros2_control_demo_hardware/demo_robot_hardware_minimal.hpp" + +using namespace ros2_control_demo_hardware_minimal; + +ros2_control_types::return_type DemoRobotHardwareMinimal::init_hardware() +{ + RCLCPP_INFO(logging_interface_->get_logger(), "Initalizing DemoRobotHardwareMinimal ...please wait..."); + + for (int i=0; i < 3; i++) + { + rclcpp::sleep_for(std::chrono::seconds(1)); + RCLCPP_INFO(logging_interface_->get_logger(), "%d seconds left...", 3-i); + } + + RCLCPP_INFO(logging_interface_->get_logger(), "DemoRobotHardwareMinimal sucessfully initalized!"); + + return ros2_control_types::ROS2C_RETURN_OK; +} + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + ros2_control_demo_hardware_minimal::DemoRobotHardwareMinimal, ros2_control_core_hardware::RobotHardware) diff --git a/ros2_control_demo_robot/CMakeLists.txt b/ros2_control_demo_robot/CMakeLists.txt index bea761dea..cd440da75 100644 --- a/ros2_control_demo_robot/CMakeLists.txt +++ b/ros2_control_demo_robot/CMakeLists.txt @@ -14,8 +14,33 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) -install(DIRECTORY config/ description/ launch/ - DESTINATION share) +find_package(control_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(ros2_control_core REQUIRED) + + +# Test node +add_executable(ros2_control_demo_robot_manager src/ros2_control_demo_robot_manager.cpp) + +target_include_directories(ros2_control_demo_robot_manager + PRIVATE include +) + +ament_target_dependencies(ros2_control_demo_robot_manager + control_msgs + rclcpp + ros2_control_core +) + +## INSTALL +install(TARGETS ros2_control_demo_robot_manager + DESTINATION lib/${PROJECT_NAME} +) + +install( + DIRECTORY config description launch + DESTINATION share/${PROJECT_NAME}/ +) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) diff --git a/ros2_control_demo_robot/config/demo_robot.yaml b/ros2_control_demo_robot/config/demo_robot.yaml index e69de29bb..894ca34f4 100644 --- a/ros2_control_demo_robot/config/demo_robot.yaml +++ b/ros2_control_demo_robot/config/demo_robot.yaml @@ -0,0 +1,50 @@ +ros2_control_demo_robot_manager: + ros__parameters: + DemoRobot: + name: "ROS2-C Demo Robot" + has_hardware: True + has_robots: False + has_sensors: True + has_tools: True + RobotHardware: + type: "ros2_control_demo_hardware/DemoRobotHardwareHeadless" + has_communication_interface: True + HardwareCommunicationInterface: + type: "ros2_control_communicaiton_interface/DemoRobotCommunicationHeadless" + + joints: [jointX, jointY] + actuators: + jointX: + name: "jointX_position_actuator" + type: "ros2_control_components/VelocityActuator" + can_read: True + n_dof: 1 + min_values: [-1] + max_values: [1] + + jointY: + name: "jointY_velocity_actuator" + type: "ros2_control_components/PositionActuator" + n_dof: 1 + min_values: [-0.5] + max_values: [1] + + sensors: + jointX: + name: "jointX_velocity_sensor" + type: "ros2_control_components/VelocitySensor" + n_dof: 1 + + jointY: + name: "jointY_velocity_sensor" + type: "ros2_control_components/VelocitySensor" + n_dof: 1 + + #tools: + #sensor_names: ["touch_sensor"] + #sensors: + #touch_sensor: + #name: "tcp_touch_sensor" + #type: "ros2_control_demo_components/TouchSensor" + #frame_id: "tcp" + #n_dof: 1 diff --git a/ros2_control_demo_robot/config/demo_robot_minimal_example.yaml b/ros2_control_demo_robot/config/demo_robot_minimal_example.yaml new file mode 100644 index 000000000..c437a15fa --- /dev/null +++ b/ros2_control_demo_robot/config/demo_robot_minimal_example.yaml @@ -0,0 +1,23 @@ +ros2_control_demo_robot_manager: + ros__parameters: + DemoRobot: + name: "ROS2-C Demo Robot Minimal" + has_hardware: True + RobotHardware: + type: "ros2_control_demo_hardware/DemoRobotHardwareMinimal" + + joints: [joint1, joint2] + actuators: + joint1: + name: "joint1_position_actuator" + type: "ros2_control_components/PositionActuator" + can_read: True + min_values: [-1] + max_values: [1] + + joint2: + name: "joint2_position_actuator" + type: "ros2_control_components/PositionActuator" + can_read: True + min_values: [-1] + max_values: [1] diff --git a/ros2_control_demo_robot/launch/demo_robot.launch.py b/ros2_control_demo_robot/launch/demo_robot.launch.py new file mode 100644 index 000000000..1ca6a14c3 --- /dev/null +++ b/ros2_control_demo_robot/launch/demo_robot.launch.py @@ -0,0 +1,49 @@ +# Copyright 2020 ROS2-Control Development Team +# +# 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. + + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + + ld = LaunchDescription() + + demo_robot_config = os.path.join( + get_package_share_directory('ros2_control_demo_robot'), + 'config', + 'demo_robot.yaml' + ) + + return LaunchDescription([ + Node( + package='ros2_control_demo_robot', + node_name='ros2_control_demo_robot_manager', + node_executable='ros2_control_demo_robot_manager', + parameters=[demo_robot_config], + output={ + 'stdout': 'screen', + 'stderr': 'screen', + }, + #log_level={ + #'logger_name': '', + #'level': 'DEBUG' + #} + ) + + ]) diff --git a/ros2_control_demo_robot/launch/demo_robot_minimal.launch.py b/ros2_control_demo_robot/launch/demo_robot_minimal.launch.py new file mode 100644 index 000000000..df30e8fd5 --- /dev/null +++ b/ros2_control_demo_robot/launch/demo_robot_minimal.launch.py @@ -0,0 +1,49 @@ +# Copyright 2020 ROS2-Control Development Team +# +# 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. + + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + + ld = LaunchDescription() + + demo_robot_config = os.path.join( + get_package_share_directory('ros2_control_demo_robot_minimal'), + 'config', + 'demo_robot_minimal_example.yaml' + ) + + return LaunchDescription([ + Node( + package='ros2_control_demo_robot', + node_name='ros2_control_demo_robot_manager', + node_executable='ros2_control_demo_robot_manager', + parameters=[demo_robot_config], + output={ + 'stdout': 'screen', + 'stderr': 'screen', + }, + #log_level={ + #'logger_name': '', + #'level': 'DEBUG' + #} + ) + + ]) diff --git a/ros2_control_demo_robot/launch/demo_robot_launch.py b/ros2_control_demo_robot/launch/demo_robot_modular.launch.py similarity index 94% rename from ros2_control_demo_robot/launch/demo_robot_launch.py rename to ros2_control_demo_robot/launch/demo_robot_modular.launch.py index 115a49f1d..3ecc8c45d 100644 --- a/ros2_control_demo_robot/launch/demo_robot_launch.py +++ b/ros2_control_demo_robot/launch/demo_robot_modular.launch.py @@ -1,4 +1,4 @@ -# Copyright 2020 ROS-Control Development Team +# Copyright 2020 ROS2-Control Development Team # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/ros2_control_demo_robot/launch/demo_robot_modular_launch.py b/ros2_control_demo_robot/launch/demo_robot_modular_launch.py deleted file mode 100644 index 115a49f1d..000000000 --- a/ros2_control_demo_robot/launch/demo_robot_modular_launch.py +++ /dev/null @@ -1,28 +0,0 @@ -# Copyright 2020 ROS-Control Development Team -# -# 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. - - -# from launch import LaunchDescription -# from launch_ros.actions import Node - - -# def generate_launch_description(): -# return LaunchDescription([ -# Node( -# package='', -# node_namespace='', -# node_executable='', -# node_name='' -# ) -# ]) diff --git a/ros2_control_demo_robot/package.xml b/ros2_control_demo_robot/package.xml index c41ee93ec..df64540e1 100644 --- a/ros2_control_demo_robot/package.xml +++ b/ros2_control_demo_robot/package.xml @@ -12,6 +12,10 @@ ament_cmake + control_msgs + rclcpp + ros2_control_core + ament_cmake_gtest diff --git a/ros2_control_demo_robot/src/ros2_control_demo_robot_manager.cpp b/ros2_control_demo_robot/src/ros2_control_demo_robot_manager.cpp new file mode 100644 index 000000000..bea5fae82 --- /dev/null +++ b/ros2_control_demo_robot/src/ros2_control_demo_robot_manager.cpp @@ -0,0 +1,83 @@ +// Copyright 2020 ROS2-Control Development Team +// +// 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. + + +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "ros2_control_core/components/robot.hpp" + + +using namespace std::chrono_literals; + + +class ROS2ControlCoreTestClass: public rclcpp::Node +{ +public: + ROS2ControlCoreTestClass(rclcpp::NodeOptions options) : Node("ros2_control_core_test_node", options) + { + robot_.reset(new ros2_control_core_components::Robot()); + robot_->configure(std::string("DemoRobot"), this->get_node_logging_interface(), this->get_node_parameters_interface(), this->get_node_services_interface()); + + parameters_client_ = std::make_shared(this); + while (!parameters_client_->wait_for_service(1s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting."); + rclcpp::shutdown(); + } + RCLCPP_INFO(this->get_logger(), "service not available, waiting again..."); + } + + robot_->init(); + + timer_ = this->create_wall_timer(1000ms, std::bind(&ROS2ControlCoreTestClass::loop, this)); + } + + void loop() + { +// robot_->read(); +// +// update_values(); + + + + robot_->recover(); + + RCLCPP_INFO(this->get_logger(), "Calling loop..."); + } + +private: + rclcpp::TimerBase::SharedPtr timer_; + std::shared_ptr robot_; + std::shared_ptr parameters_client_; + control_msgs::msg::InterfaceValue values_; + +// void update_values() +// { +// +// } +}; + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; +// options.allow_undeclared_parameters(true); +// options.automatically_declare_parameters_from_overrides(true); + rclcpp::spin(std::make_shared(options)); + rclcpp::shutdown(); + return 0; +} diff --git a/ros2_control_demo_robot/src/ros2_control_demo_robot_manager_minimal.cpp b/ros2_control_demo_robot/src/ros2_control_demo_robot_manager_minimal.cpp new file mode 100644 index 000000000..bea5fae82 --- /dev/null +++ b/ros2_control_demo_robot/src/ros2_control_demo_robot_manager_minimal.cpp @@ -0,0 +1,83 @@ +// Copyright 2020 ROS2-Control Development Team +// +// 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. + + +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "ros2_control_core/components/robot.hpp" + + +using namespace std::chrono_literals; + + +class ROS2ControlCoreTestClass: public rclcpp::Node +{ +public: + ROS2ControlCoreTestClass(rclcpp::NodeOptions options) : Node("ros2_control_core_test_node", options) + { + robot_.reset(new ros2_control_core_components::Robot()); + robot_->configure(std::string("DemoRobot"), this->get_node_logging_interface(), this->get_node_parameters_interface(), this->get_node_services_interface()); + + parameters_client_ = std::make_shared(this); + while (!parameters_client_->wait_for_service(1s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting."); + rclcpp::shutdown(); + } + RCLCPP_INFO(this->get_logger(), "service not available, waiting again..."); + } + + robot_->init(); + + timer_ = this->create_wall_timer(1000ms, std::bind(&ROS2ControlCoreTestClass::loop, this)); + } + + void loop() + { +// robot_->read(); +// +// update_values(); + + + + robot_->recover(); + + RCLCPP_INFO(this->get_logger(), "Calling loop..."); + } + +private: + rclcpp::TimerBase::SharedPtr timer_; + std::shared_ptr robot_; + std::shared_ptr parameters_client_; + control_msgs::msg::InterfaceValue values_; + +// void update_values() +// { +// +// } +}; + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; +// options.allow_undeclared_parameters(true); +// options.automatically_declare_parameters_from_overrides(true); + rclcpp::spin(std::make_shared(options)); + rclcpp::shutdown(); + return 0; +}