Unverified Commit 069aea99 authored by Denis Štogl's avatar Denis Štogl Committed by GitHub
Browse files

Fix formatting of the repo. (#82)

* add pre commit configuration

* Updated clang-format

* Added corrected files

* Revert to master

* Starting new CI configuration with pre-commit-hooks

* Add clang format config - copied from ament-clang-format

* Update action setup

* Update version of hooks

* Remove double pre-commit configuration

* Update deprecated hook.

* Update linters and formating configuration

* Braces are allways in the new line

* Updatd all line lengths to 100

* Add cpp lint option to ignore braces after else

* Put clang-format first to reduce cpplint and cppcheck errors

* Update build times and small error in setup

* Reformat all repo files

* Update configuration to ignore chnagelogs

* Update python line length to 99
parent 6471232a
......@@ -42,7 +42,7 @@ repos:
rev: 20.8b1
hooks:
- id: black
args: ["--line-length=100"]
args: ["--line-length=99"]
# PEP 257
- repo: https://github.com/FalconSocial/pre-commit-mirrors-pep257
......@@ -105,7 +105,7 @@ repos:
stages: [commit]
entry: ament_lint_cmake
language: system
files: CMakeLists.txt$
files: CMakeLists\.txt$
# Copyright
- repo: local
......@@ -123,11 +123,13 @@ repos:
hooks:
- id: doc8
args: ['--max-line-length=100', '--ignore=D001']
exclude: CHANGELOG\.rst$
- repo: https://github.com/pre-commit/pygrep-hooks
rev: v1.8.0
hooks:
- id: rst-backticks
exclude: CHANGELOG\.rst$
- id: rst-directive-colons
- id: rst-inline-touching-normal
......
......@@ -10,7 +10,7 @@ information to effectively respond to your bug report or contribution.
## Reporting Bugs/Feature Requests
We welcome you to use the GitHub issue tracker to report bugs or suggest features.
When filing an issue, please check [existing open][issues], or [recently closed][closed-issues], issues to make sure
When filing an issue, please check [existing open][issues], or [recently closed][closed-issues], issues to make sure
somebody else hasn't already reported the issue.
Please try to include as much information as you can. Details like these are incredibly useful:
......@@ -45,7 +45,7 @@ GitHub provides additional documentation on [forking a repository](https://help.
## Finding contributions to work on
Looking at the existing issues is a great way to find something to contribute on.
As this project, by default, uses the default GitHub issue labels
(enhancement/bug/duplicate/help wanted/invalid/question/wontfix), looking at any ['help wanted'][help-wanted] issues
(enhancement/bug/duplicate/help wanted/invalid/question/wontfix), looking at any ['help wanted'][help-wanted] issues
is a great place to start.
......
......@@ -30,7 +30,7 @@ This repository demonstrates the following `ros2_control` concepts:
* Creating of `*HardwareInterface` for a System, Sensor, and Actuator.
* Creating a robot description in the form of URDF files
* Loading the configuration and starting a robot using launch files
* Loading the configuration and starting a robot using launch files
* Control of two joints of *RRBot*
* Using simulated robots and starting `ros_control` with Gazebo simulator
* Implementing of controller switching strategy for a robot
......@@ -62,20 +62,20 @@ git clone https://github.com/ros-controls/ros2_control_demos
```
* Build everything, e.g. with:
```
```
colcon build --symlink-install
```
* Do not forget to source `setup.bash` from the `install` folder!
# Getting Started with ros2_control
Each of the described example cases from the roadmap has its own launch and URDF file.
## Starting example robots
Each example is started with a single launch file which starts up the robot hardware, loads controller configurations and it also opens `rviz2`.
Each example is started with a single launch file which starts up the robot hardware, loads controller configurations and it also opens `rviz2`.
The `rviz2` setup can be recreated following these steps:
......
This source diff could not be displayed because it is too large. You can view the blob instead.
This diff is collapsed.
......@@ -12,7 +12,6 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef ROS2_CONTROL_DEMO_HARDWARE__RRBOT_SYSTEM_POSITION_ONLY_HPP_
#define ROS2_CONTROL_DEMO_HARDWARE__RRBOT_SYSTEM_POSITION_ONLY_HPP_
......@@ -21,9 +20,9 @@
#include <vector>
#include "hardware_interface/base_interface.hpp"
#include "hardware_interface/system_interface.hpp"
#include "hardware_interface/handle.hpp"
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/system_interface.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "hardware_interface/types/hardware_interface_status_values.hpp"
#include "rclcpp/macros.hpp"
......@@ -31,8 +30,8 @@
namespace ros2_control_demo_hardware
{
class RRBotSystemPositionOnlyHardware : public
hardware_interface::BaseInterface<hardware_interface::SystemInterface>
class RRBotSystemPositionOnlyHardware
: public hardware_interface::BaseInterface<hardware_interface::SystemInterface>
{
public:
RCLCPP_SHARED_PTR_DEFINITIONS(RRBotSystemPositionOnlyHardware);
......
......@@ -25,11 +25,11 @@
namespace ros2_control_demo_hardware
{
hardware_interface::return_type RRBotSystemPositionOnlyHardware::configure(
const hardware_interface::HardwareInfo & info)
{
if (configure_default(info) != hardware_interface::return_type::OK) {
if (configure_default(info) != hardware_interface::return_type::OK)
{
return hardware_interface::return_type::ERROR;
}
......@@ -39,39 +39,42 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::configure(
hw_states_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
hw_commands_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
for (const hardware_interface::ComponentInfo & joint : info_.joints) {
for (const hardware_interface::ComponentInfo & joint : info_.joints)
{
// RRBotSystemPositionOnly has exactly one state and command interface on each joint
if (joint.command_interfaces.size() != 1) {
if (joint.command_interfaces.size() != 1)
{
RCLCPP_FATAL(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
"Joint '%s' has %d command interfaces found. 1 expected.",
joint.name.c_str(), joint.command_interfaces.size());
"Joint '%s' has %d command interfaces found. 1 expected.", joint.name.c_str(),
joint.command_interfaces.size());
return hardware_interface::return_type::ERROR;
}
if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) {
if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION)
{
RCLCPP_FATAL(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
"Joint '%s' have %s command interfaces found. '%s' expected.",
joint.name.c_str(), joint.command_interfaces[0].name.c_str(),
hardware_interface::HW_IF_POSITION);
"Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(),
joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION);
return hardware_interface::return_type::ERROR;
}
if (joint.state_interfaces.size() != 1) {
if (joint.state_interfaces.size() != 1)
{
RCLCPP_FATAL(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
"Joint '%s' has %d state interface. 1 expected.",
joint.name.c_str(), joint.state_interfaces.size());
"Joint '%s' has %d state interface. 1 expected.", joint.name.c_str(),
joint.state_interfaces.size());
return hardware_interface::return_type::ERROR;
}
if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) {
if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION)
{
RCLCPP_FATAL(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
"Joint '%s' have %s state interface. '%s' expected.",
joint.name.c_str(), joint.state_interfaces[0].name.c_str(),
hardware_interface::HW_IF_POSITION);
"Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(),
joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION);
return hardware_interface::return_type::ERROR;
}
}
......@@ -84,10 +87,10 @@ std::vector<hardware_interface::StateInterface>
RRBotSystemPositionOnlyHardware::export_state_interfaces()
{
std::vector<hardware_interface::StateInterface> state_interfaces;
for (uint i = 0; i < info_.joints.size(); i++) {
state_interfaces.emplace_back(
hardware_interface::StateInterface(
info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_[i]));
for (uint i = 0; i < info_.joints.size(); i++)
{
state_interfaces.emplace_back(hardware_interface::StateInterface(
info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_[i]));
}
return state_interfaces;
......@@ -97,35 +100,37 @@ std::vector<hardware_interface::CommandInterface>
RRBotSystemPositionOnlyHardware::export_command_interfaces()
{
std::vector<hardware_interface::CommandInterface> command_interfaces;
for (uint i = 0; i < info_.joints.size(); i++) {
command_interfaces.emplace_back(
hardware_interface::CommandInterface(
info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i]));
for (uint i = 0; i < info_.joints.size(); i++)
{
command_interfaces.emplace_back(hardware_interface::CommandInterface(
info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i]));
}
return command_interfaces;
}
hardware_interface::return_type RRBotSystemPositionOnlyHardware::start()
{
RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
"Starting ...please wait...");
RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Starting ...please wait...");
for (int i = 0; i <= hw_start_sec_; i++) {
for (int i = 0; i <= hw_start_sec_; i++)
{
rclcpp::sleep_for(std::chrono::seconds(1));
RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
"%.1f seconds left...", hw_start_sec_ - i);
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "%.1f seconds left...",
hw_start_sec_ - i);
}
// set some default values when starting the first time
for (uint i = 0; i < hw_states_.size(); i++) {
if (std::isnan(hw_states_[i])) {
for (uint i = 0; i < hw_states_.size(); i++)
{
if (std::isnan(hw_states_[i]))
{
hw_states_[i] = 0;
hw_commands_[i] = 0;
} else {
}
else
{
hw_commands_[i] = hw_states_[i];
}
}
......@@ -133,69 +138,61 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::start()
status_ = hardware_interface::status::STARTED;
RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
"System Sucessfully started!");
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "System Successfully started!");
return hardware_interface::return_type::OK;
}
hardware_interface::return_type RRBotSystemPositionOnlyHardware::stop()
{
RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
"Stopping ...please wait...");
RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Stopping ...please wait...");
for (int i = 0; i <= hw_stop_sec_; i++) {
for (int i = 0; i <= hw_stop_sec_; i++)
{
rclcpp::sleep_for(std::chrono::seconds(1));
RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
"%.1f seconds left...", hw_stop_sec_ - i);
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "%.1f seconds left...",
hw_stop_sec_ - i);
}
status_ = hardware_interface::status::STOPPED;
RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
"System sucessfully stopped!");
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "System successfully stopped!");
return hardware_interface::return_type::OK;
}
hardware_interface::return_type RRBotSystemPositionOnlyHardware::read()
{
RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
"Reading...");
RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Reading...");
for (uint i = 0; i < hw_states_.size(); i++) {
for (uint i = 0; i < hw_states_.size(); i++)
{
// Simulate RRBot's movement
hw_states_[i] = hw_states_[i] + (hw_commands_[i] - hw_states_[i]) / hw_slowdown_;
RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
"Got state %.5f for joint %d!", hw_states_[i], i);
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Got state %.5f for joint %d!",
hw_states_[i], i);
}
RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
"Joints sucessfully read!");
RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Joints successfully read!");
return hardware_interface::return_type::OK;
}
hardware_interface::return_type RRBotSystemPositionOnlyHardware::write()
{
RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
"Writing...");
RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Writing...");
for (uint i = 0; i < hw_commands_.size(); i++) {
for (uint i = 0; i < hw_commands_.size(); i++)
{
// Simulate sending commands to the hardware
RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
"Got command %.5f for joint %d!", hw_commands_[i], i);
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Got command %.5f for joint %d!",
hw_commands_[i], i);
}
RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
"Joints sucessfully written!");
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Joints successfully written!");
return hardware_interface::return_type::OK;
}
......@@ -205,6 +202,4 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::write()
#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(
ros2_control_demo_hardware::RRBotSystemPositionOnlyHardware,
hardware_interface::SystemInterface
)
ros2_control_demo_hardware::RRBotSystemPositionOnlyHardware, hardware_interface::SystemInterface)
......@@ -42,4 +42,3 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc
</material>
</robot>
......@@ -26,49 +26,49 @@ def generate_launch_description():
# Get URDF via xacro
robot_description_path = os.path.join(
get_package_share_directory('ros2_control_demo_robot'),
'description',
'rrbot_system_position_only.urdf.xacro')
robot_description_config = xacro.process_file(robot_description_path,
mappings={'slowdown': '3.0'})
robot_description = {'robot_description': robot_description_config.toxml()}
get_package_share_directory("ros2_control_demo_robot"),
"description",
"rrbot_system_position_only.urdf.xacro",
)
robot_description_config = xacro.process_file(
robot_description_path, mappings={"slowdown": "3.0"}
)
robot_description = {"robot_description": robot_description_config.toxml()}
rrbot_forward_controller = os.path.join(
get_package_share_directory('ros2_control_demo_robot'),
'config',
'rrbot_controllers.yaml'
)
get_package_share_directory("ros2_control_demo_robot"), "config", "rrbot_controllers.yaml"
)
rviz_config_file = os.path.join(
get_package_share_directory('ros2_control_demo_robot'),
'rviz',
'rrbot.rviz'
)
get_package_share_directory("ros2_control_demo_robot"), "rviz", "rrbot.rviz"
)
control_node = Node(
package='controller_manager',
executable='ros2_control_node',
parameters=[robot_description, rrbot_forward_controller],
output={
'stdout': 'screen',
'stderr': 'screen',
package="controller_manager",
executable="ros2_control_node",
parameters=[robot_description, rrbot_forward_controller],
output={
"stdout": "screen",
"stderr": "screen",
},
)
robot_state_publisher_node = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
output='both',
parameters=[robot_description]
package="robot_state_publisher",
executable="robot_state_publisher",
output="both",
parameters=[robot_description],
)
rviz_node = Node(
package='rviz2',
executable='rviz2',
name='rviz2',
output='log',
arguments=['-d', rviz_config_file],
package="rviz2",
executable="rviz2",
name="rviz2",
output="log",
arguments=["-d", rviz_config_file],
)
return LaunchDescription([
control_node,
robot_state_publisher_node,
rviz_node,
])
return LaunchDescription(
[
control_node,
robot_state_publisher_node,
rviz_node,
]
)
......@@ -23,20 +23,21 @@ from launch_ros.actions import Node
def generate_launch_description():
position_goals = os.path.join(
get_package_share_directory('ros2_control_test_nodes'),
'configs',
'rrbot_forward_position_publisher.yaml'
)
return LaunchDescription([
Node(
package='ros2_control_test_nodes',
executable='publisher_forward_position_controller',
parameters=[position_goals],
output={
'stdout': 'screen',
'stderr': 'screen',
},
)
])
get_package_share_directory("ros2_control_test_nodes"),
"configs",
"rrbot_forward_position_publisher.yaml",
)
return LaunchDescription(
[
Node(
package="ros2_control_test_nodes",
executable="publisher_forward_position_controller",
parameters=[position_goals],
output={
"stdout": "screen",
"stderr": "screen",
},
)
]
)
......@@ -26,38 +26,39 @@ def generate_launch_description():
# Get URDF via xacro
robot_description_path = os.path.join(
get_package_share_directory('ros2_control_demo_robot'),
'description',
'rrbot_system_position_only.urdf.xacro')
get_package_share_directory("ros2_control_demo_robot"),
"description",
"rrbot_system_position_only.urdf.xacro",
)
robot_description_config = xacro.process_file(robot_description_path)
robot_description = {'robot_description': robot_description_config.toxml()}
robot_description = {"robot_description": robot_description_config.toxml()}
rviz_config_file = os.path.join(
get_package_share_directory('ros2_control_demo_robot'),
'rviz',
'rrbot.rviz'
)
get_package_share_directory("ros2_control_demo_robot"), "rviz", "rrbot.rviz"
)
joint_state_publisher_node = Node(
package='joint_state_publisher_gui',
executable='joint_state_publisher_gui',
package="joint_state_publisher_gui",
executable="joint_state_publisher_gui",
)
robot_state_publisher_node = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
output='both',
parameters=[robot_description]
package="robot_state_publisher",
executable="robot_state_publisher",
output="both",
parameters=[robot_description],
)
rviz_node = Node(
package='rviz2',
executable='rviz2',
name='rviz2',
output='log',
arguments=['-d', rviz_config_file],
package="rviz2",
executable="rviz2",
name="rviz2",
output="log",
arguments=["-d", rviz_config_file],
)
return LaunchDescription([
joint_state_publisher_node,
robot_state_publisher_node,
rviz_node,
])
return LaunchDescription(
[
joint_state_publisher_node,
robot_state_publisher_node,
rviz_node,
]
)
......@@ -13,13 +13,9 @@
<depend>std_msgs</depend>
<depend>trajectory_msgs</depend>
<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>
<test_depend>ament_pep257</test_depend>
<test_depend>python3-pytest</test_depend>
<export>
<build_type>ament_python</build_type>
</export>
</package>
......@@ -19,18 +19,17 @@ from std_msgs.msg import Float64MultiArray
class PublisherForwardPosition(Node):
def __init__(self):
super().__init__('publisher_forward_position_controller')