From 0c8b6bda80b3f6eabb5d5b758ecacaca6de34032 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 1 Aug 2023 10:52:02 +0000 Subject: [PATCH] Apply pre-commit --- .../include/r6bot_hardware/r6bot_hardware.hpp | 19 +++++++++++++++++- r6bot/hardware_driver/src/r6bot_hardware.cpp | 16 ++++++++++++++- .../r6bot_controller/r6bot_controller.hpp | 20 ++++++++++++++++++- .../launch/r6bot_controller.launch.py | 14 +++++++++++++ .../r6bot_controller/src/r6bot_controller.cpp | 19 ++++++++++++++++-- .../launch/view_r6bot.launch.py | 14 +++++++++++++ .../launch/send_trajectory.launch.py | 14 +++++++++++++ .../src/send_trajectory.cpp | 20 ++++++++++++++++--- 8 files changed, 128 insertions(+), 8 deletions(-) diff --git a/r6bot/hardware_driver/include/r6bot_hardware/r6bot_hardware.hpp b/r6bot/hardware_driver/include/r6bot_hardware/r6bot_hardware.hpp index 49010297d..b9741a547 100644 --- a/r6bot/hardware_driver/include/r6bot_hardware/r6bot_hardware.hpp +++ b/r6bot/hardware_driver/include/r6bot_hardware/r6bot_hardware.hpp @@ -1,4 +1,19 @@ -#pragma once +// Copyright 2023 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 R6BOT_HARDWARE__R6BOT_HARDWARE_HPP_ +#define R6BOT_HARDWARE__R6BOT_HARDWARE_HPP_ #include "string" #include "unordered_map" @@ -43,3 +58,5 @@ class HARDWARE_INTERFACE_PUBLIC RobotSystem : public hardware_interface::SystemI }; } // namespace r6bot_hardware + +#endif // R6BOT_HARDWARE__R6BOT_HARDWARE_HPP_ diff --git a/r6bot/hardware_driver/src/r6bot_hardware.cpp b/r6bot/hardware_driver/src/r6bot_hardware.cpp index a4d5f99d9..c4d4ccf18 100644 --- a/r6bot/hardware_driver/src/r6bot_hardware.cpp +++ b/r6bot/hardware_driver/src/r6bot_hardware.cpp @@ -1,3 +1,17 @@ +// Copyright 2023 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 "r6bot_hardware/r6bot_hardware.hpp" #include #include @@ -86,7 +100,7 @@ std::vector RobotSystem::export_command_in return_type RobotSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { - // TODO set sensor_states_ values from subscriber + // TODO(pac48) set sensor_states_ values from subscriber for (auto i = 0ul; i < joint_velocities_command_.size(); i++) { diff --git a/r6bot/r6bot_controller/include/r6bot_controller/r6bot_controller.hpp b/r6bot/r6bot_controller/include/r6bot_controller/r6bot_controller.hpp index b244400e4..45fea3d44 100644 --- a/r6bot/r6bot_controller/include/r6bot_controller/r6bot_controller.hpp +++ b/r6bot/r6bot_controller/include/r6bot_controller/r6bot_controller.hpp @@ -1,9 +1,25 @@ -#pragma once +// Copyright 2023 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 R6BOT_CONTROLLER__R6BOT_CONTROLLER_HPP_ +#define R6BOT_CONTROLLER__R6BOT_CONTROLLER_HPP_ #include #include #include #include +#include #include #include @@ -102,3 +118,5 @@ class RobotController : public controller_interface::ControllerInterface }; } // namespace r6bot_controller + +#endif // R6BOT_CONTROLLER__R6BOT_CONTROLLER_HPP_ diff --git a/r6bot/r6bot_controller/launch/r6bot_controller.launch.py b/r6bot/r6bot_controller/launch/r6bot_controller.launch.py index 8b2be684a..bc4e06fb3 100644 --- a/r6bot/r6bot_controller/launch/r6bot_controller.launch.py +++ b/r6bot/r6bot_controller/launch/r6bot_controller.launch.py @@ -1,3 +1,17 @@ +# Copyright 2023 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. + from launch import LaunchDescription from launch.actions import RegisterEventHandler from launch.event_handlers import OnProcessExit diff --git a/r6bot/r6bot_controller/src/r6bot_controller.cpp b/r6bot/r6bot_controller/src/r6bot_controller.cpp index 7ad55df10..680e526aa 100644 --- a/r6bot/r6bot_controller/src/r6bot_controller.cpp +++ b/r6bot/r6bot_controller/src/r6bot_controller.cpp @@ -1,3 +1,17 @@ +// Copyright 2023 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 "r6bot_controller/r6bot_controller.hpp" #include @@ -66,7 +80,8 @@ controller_interface::InterfaceConfiguration RobotController::state_interface_co controller_interface::CallbackReturn RobotController::on_configure(const rclcpp_lifecycle::State &) { auto callback = - [this](const std::shared_ptr traj_msg) -> void { + [this](const std::shared_ptr traj_msg) -> void + { traj_msg_external_point_ptr_.writeFromNonRT(traj_msg); new_msg_ = true; }; @@ -126,7 +141,7 @@ void interpolate_trajectory_point( double total_time = last_time.sec + last_time.nanosec * 1E-9; size_t ind = cur_time.seconds() * (traj_len / total_time); - ind = std::min(ind, (size_t)traj_len - 2); + ind = std::min(ind, static_cast(traj_len) - 2); double delta = cur_time.seconds() - ind * (total_time / traj_len); interpolate_point(traj_msg.points[ind], traj_msg.points[ind + 1], point_interp, delta); } diff --git a/r6bot/r6bot_description/launch/view_r6bot.launch.py b/r6bot/r6bot_description/launch/view_r6bot.launch.py index 45084318f..044acbf37 100644 --- a/r6bot/r6bot_description/launch/view_r6bot.launch.py +++ b/r6bot/r6bot_description/launch/view_r6bot.launch.py @@ -1,3 +1,17 @@ +# Copyright 2023 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. + from launch import LaunchDescription from launch.substitutions import Command, FindExecutable, PathJoinSubstitution from launch_ros.actions import Node diff --git a/r6bot/reference_generator/launch/send_trajectory.launch.py b/r6bot/reference_generator/launch/send_trajectory.launch.py index cd9dca29d..49bd1ab55 100644 --- a/r6bot/reference_generator/launch/send_trajectory.launch.py +++ b/r6bot/reference_generator/launch/send_trajectory.launch.py @@ -1,3 +1,17 @@ +# Copyright 2023 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. + from launch import LaunchDescription from launch.substitutions import Command, FindExecutable, PathJoinSubstitution from launch_ros.actions import Node diff --git a/r6bot/reference_generator/src/send_trajectory.cpp b/r6bot/reference_generator/src/send_trajectory.cpp index 33f19d72b..8222a7f48 100644 --- a/r6bot/reference_generator/src/send_trajectory.cpp +++ b/r6bot/reference_generator/src/send_trajectory.cpp @@ -1,3 +1,17 @@ +// Copyright 2023 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 @@ -30,7 +44,6 @@ int main(int argc, char ** argv) auto joint_velocities = KDL::JntArray(chain.getNrOfJoints()); auto twist = KDL::Twist(); // create KDL solvers - auto fk_pos_solver_ = std::make_shared(chain); auto ik_vel_solver_ = std::make_shared(chain, 0.0000001); trajectory_msgs::msg::JointTrajectory trajectory_msg; @@ -76,8 +89,9 @@ int main(int argc, char ** argv) // set timing information trajectory_point_msg.time_from_start.sec = i / loop_rate; - trajectory_point_msg.time_from_start.nanosec = - 1E9 / loop_rate * (double)(i - loop_rate * (i / loop_rate)); + trajectory_point_msg.time_from_start.nanosec = static_cast( + 1E9 / loop_rate * + static_cast(t - loop_rate * (i / loop_rate))); // implicit integer division trajectory_msg.points.push_back(trajectory_point_msg); }