Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
45 changes: 45 additions & 0 deletions .github/workflows/lint.yaml
Original file line number Diff line number Diff line change
@@ -0,0 +1,45 @@
name: Lint system modes
on:
pull_request:

jobs:
ament_xmllint:
name: ament_xmllint
runs-on: ubuntu-18.04
steps:
- uses: actions/checkout@master
- uses: ros-tooling/setup-ros@master
- uses: ros-tooling/action-ros-lint@master
with:
linter: xmllint
package-name: system_modes system_modes_examples

ament_lint_cpp:
name: ament_${{ matrix.linter }}
runs-on: ubuntu-18.04
strategy:
fail-fast: false
matrix:
linter: [cppcheck, cpplint, uncrustify]
steps:
- uses: actions/checkout@master
- uses: ros-tooling/setup-ros@master
- uses: ros-tooling/action-ros-lint@master
with:
linter: ${{ matrix.linter }}
package-name: system_modes system_modes_examples

ament_lint_py:
name: ament_${{ matrix.linter }}
runs-on: ubuntu-18.04
strategy:
fail-fast: false
matrix:
linter: [flake8, pep257]
steps:
- uses: actions/checkout@master
- uses: ros-tooling/setup-ros@master
- uses: ros-tooling/action-ros-lint@master
with:
linter: ${{ matrix.linter }}
package-name: system_modes system_modes_examples
21 changes: 21 additions & 0 deletions .github/workflows/test.yaml
Original file line number Diff line number Diff line change
@@ -0,0 +1,21 @@
name: Test diagnostics
on:
pull_request:
push:
branches:
- master

jobs:
build_and_test:
runs-on: ${{ matrix.os }}
strategy:
fail-fast: false
matrix:
os: [ubuntu-18.04]
steps:
- uses: ros-tooling/setup-ros@master
- uses: ros-tooling/action-ros-ci@master
with:
package-name: system_modes system_modes_examples
colcon-mixin-name: coverage-gcc
colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml
13 changes: 13 additions & 0 deletions system_modes_examples/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -74,4 +74,17 @@ install(FILES example_modes.yaml
)
install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}/)

if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
find_package(ament_cmake_gtest REQUIRED)
find_package(ament_cmake_gmock REQUIRED)
# the following line skips the linter which checks for copyrights
# remove the line when a copyright and license is present in all source files
set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# remove the line when this package is a git repo
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()

ament_package()
4 changes: 2 additions & 2 deletions system_modes_examples/launch/drive_base.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@
def generate_launch_description():
launch.actions.DeclareLaunchArgument('modelfile', description='Path to modelfile')
modelfile = (ament_index_python.packages.get_package_share_directory('system_modes_examples') +
'/example_modes.yaml')
'/example_modes.yaml')

node = launch_ros.actions.Node(
package='system_modes_examples',
Expand All @@ -36,4 +36,4 @@ def generate_launch_description():
description = launch.LaunchDescription()
description.add_action(node)

return description
return description
5 changes: 2 additions & 3 deletions system_modes_examples/launch/example_system.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -19,12 +19,11 @@
import launch.actions
import launch.launch_description_sources
import launch.substitutions
import launch_ros


def generate_launch_description():
modelfile = (ament_index_python.packages.get_package_share_directory('system_modes_examples') +
'/example_modes.yaml')
'/example_modes.yaml')

mode_manager = launch.actions.IncludeLaunchDescription(
launch.launch_description_sources.PythonLaunchDescriptionSource(
Expand All @@ -49,4 +48,4 @@ def generate_launch_description():
description.add_action(drive_base)
description.add_action(manipulator)

return description
return description
4 changes: 2 additions & 2 deletions system_modes_examples/launch/manipulator.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@
def generate_launch_description():
launch.actions.DeclareLaunchArgument('modelfile', description='Path to modelfile')
modelfile = (ament_index_python.packages.get_package_share_directory('system_modes_examples') +
'/example_modes.yaml')
'/example_modes.yaml')

node = launch_ros.actions.Node(
package='system_modes_examples',
Expand All @@ -36,4 +36,4 @@ def generate_launch_description():
description = launch.LaunchDescription()
description.add_action(node)

return description
return description
9 changes: 9 additions & 0 deletions system_modes_examples/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,15 @@
<depend>system_modes</depend>
<depend>libboost-program-options-dev</depend>

<test_depend>ament_cmake_gtest</test_depend>
<test_depend>ament_cmake_gmock</test_depend>
<test_depend>ament_cmake_pep257</test_depend>
<test_depend>ament_cmake_flake8</test_depend>
<test_depend>ament_cmake_cpplint</test_depend>
<test_depend>ament_cmake_cppcheck</test_depend>
<test_depend>ament_cmake_uncrustify</test_depend>
<test_depend>ament_lint_auto</test_depend>

<export>
<build_type>ament_cmake</build_type>
</export>
Expand Down
49 changes: 24 additions & 25 deletions system_modes_examples/src/drive_base.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -18,11 +18,8 @@
#include <lifecycle_msgs/msg/state.hpp>
#include <lifecycle_msgs/msg/transition.hpp>

#include <chrono>
#include <iostream>
#include <memory>
#include <string>
#include <thread>
#include <vector>

using lifecycle_msgs::msg::State;
using lifecycle_msgs::msg::Transition;
Expand All @@ -39,34 +36,36 @@ namespace examples
class DriveBase : public LifecycleNode
{
public:
explicit DriveBase()
DriveBase()
: LifecycleNode("drive_base")
{
RCLCPP_INFO(get_logger(), "Constructed lifecycle node '%s'", this->get_name());

// Parameter declaration
this->declare_parameter("max_speed",
this->declare_parameter(
"max_speed",
rclcpp::ParameterValue(rclcpp::PARAMETER_NOT_SET),
rcl_interfaces::msg::ParameterDescriptor());
this->declare_parameter("controller",
this->declare_parameter(
"controller",
rclcpp::ParameterValue(rclcpp::PARAMETER_NOT_SET),
rcl_interfaces::msg::ParameterDescriptor());

auto param_change_callback =
[this](std::vector<rclcpp::Parameter> parameters) -> rcl_interfaces::msg::SetParametersResult
{
auto result = rcl_interfaces::msg::SetParametersResult();
result.successful = true;
for (auto parameter : parameters) {
RCLCPP_INFO(this->get_logger(),
"parameter '%s' is now: %s",
parameter.get_name().c_str(),
parameter.value_to_string().c_str()
);
}
return result;
};
[this](std::vector<rclcpp::Parameter> parameters) -> rcl_interfaces::msg::SetParametersResult
{
auto result = rcl_interfaces::msg::SetParametersResult();
result.successful = true;
for (auto parameter : parameters) {
RCLCPP_INFO(
this->get_logger(),
"parameter '%s' is now: %s",
parameter.get_name().c_str(),
parameter.value_to_string().c_str());
}
return result;
};

param_change_callback_handle_ = this->add_on_set_parameters_callback(param_change_callback);
}

Expand All @@ -79,31 +78,31 @@ class DriveBase : public LifecycleNode
RCLCPP_INFO(get_logger(), "on_configure()", this->get_name());

return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
};
}

rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
on_activate(const rclcpp_lifecycle::State &)
{
RCLCPP_INFO(get_logger(), "on_activate()", this->get_name());

return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
};
}

rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
on_deactivate(const rclcpp_lifecycle::State &)
{
RCLCPP_INFO(get_logger(), "on_deactivate()", this->get_name());

return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
};
}

rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
on_cleanup(const rclcpp_lifecycle::State &)
{
RCLCPP_INFO(get_logger(), "on_cleanup()", this->get_name());

return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
};
}

private:
rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr param_change_callback_handle_;
Expand Down
46 changes: 22 additions & 24 deletions system_modes_examples/src/manipulator.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -18,11 +18,8 @@
#include <lifecycle_msgs/msg/state.hpp>
#include <lifecycle_msgs/msg/transition.hpp>

#include <chrono>
#include <iostream>
#include <memory>
#include <string>
#include <thread>
#include <vector>

using lifecycle_msgs::msg::State;
using lifecycle_msgs::msg::Transition;
Expand All @@ -39,31 +36,32 @@ namespace examples
class Manipulator : public LifecycleNode
{
public:
explicit Manipulator()
Manipulator()
: LifecycleNode("manipulator")
{
RCLCPP_INFO(get_logger(), "Constructed lifecycle node '%s'", this->get_name());

// Parameter declaration
this->declare_parameter("max_torque",
this->declare_parameter(
"max_torque",
rclcpp::ParameterValue(rclcpp::PARAMETER_NOT_SET),
rcl_interfaces::msg::ParameterDescriptor());

auto param_change_callback =
[this](std::vector<rclcpp::Parameter> parameters) -> rcl_interfaces::msg::SetParametersResult
{
auto result = rcl_interfaces::msg::SetParametersResult();
result.successful = true;
for (auto parameter : parameters) {
RCLCPP_INFO(this->get_logger(),
"parameter '%s' is now: %s",
parameter.get_name().c_str(),
parameter.value_to_string().c_str()
);
}
return result;
};
[this](std::vector<rclcpp::Parameter> parameters) -> rcl_interfaces::msg::SetParametersResult
{
auto result = rcl_interfaces::msg::SetParametersResult();
result.successful = true;
for (auto parameter : parameters) {
RCLCPP_INFO(
this->get_logger(),
"parameter '%s' is now: %s",
parameter.get_name().c_str(),
parameter.value_to_string().c_str());
}
return result;
};

param_change_callback_handle_ = this->add_on_set_parameters_callback(param_change_callback);
}

Expand All @@ -76,31 +74,31 @@ class Manipulator : public LifecycleNode
RCLCPP_INFO(get_logger(), "on_configure()", this->get_name());

return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
};
}

rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
on_activate(const rclcpp_lifecycle::State &)
{
RCLCPP_INFO(get_logger(), "on_activate()", this->get_name());

return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
};
}

rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
on_deactivate(const rclcpp_lifecycle::State &)
{
RCLCPP_INFO(get_logger(), "on_deactivate()", this->get_name());

return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
};
}

rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
on_cleanup(const rclcpp_lifecycle::State &)
{
RCLCPP_INFO(get_logger(), "on_cleanup()", this->get_name());

return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
};
}

private:
rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr param_change_callback_handle_;
Expand Down