Skip to content

Commit

Permalink
Merge branch 'master' into variants-ex14
Browse files Browse the repository at this point in the history
  • Loading branch information
christophfroehlich authored Dec 25, 2024
2 parents 1148fef + 27f5291 commit f877a00
Show file tree
Hide file tree
Showing 9 changed files with 37 additions and 32 deletions.
11 changes: 7 additions & 4 deletions example_11/bringup/launch/carlikebot.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -77,9 +77,6 @@ def generate_launch_description():
executable="ros2_control_node",
parameters=[robot_controllers],
output="both",
remappings=[
("/bicycle_steering_controller/tf_odometry", "/tf"),
],
condition=IfCondition(remap_odometry_tf),
)
control_node = Node(
Expand Down Expand Up @@ -114,7 +111,13 @@ def generate_launch_description():
robot_bicycle_controller_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=["bicycle_steering_controller", "--param-file", robot_controllers],
arguments=[
"bicycle_steering_controller",
"--param-file",
robot_controllers,
"--controller-ros-args",
"-r /bicycle_steering_controller/tf_odometry:=/tf",
],
)

# Delay rviz start after `joint_state_broadcaster`
Expand Down
10 changes: 10 additions & 0 deletions example_11/test/test_carlikebot_launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,7 @@
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_testing.actions import ReadyToTest
from launch_testing_ros import WaitForTopics

# import launch_testing.markers
import rclpy
Expand All @@ -46,6 +47,8 @@
check_node_running,
)

from tf2_msgs.msg import TFMessage


# Executes the given launch file and checks if all nodes can be started
@pytest.mark.rostest
Expand Down Expand Up @@ -98,6 +101,13 @@ def test_check_if_msgs_published(self):
],
)

def test_remapped_topic(self):
# we don't want to implement a tf lookup here
# so just check if the unmapped topic is not published
old_topic = "/bicycle_steering_controller/tf_odometry"
wait_for_topics = WaitForTopics([(old_topic, TFMessage)])
assert not wait_for_topics.wait(), f"Topic '{old_topic}' found, but should be remapped!"


# TODO(anyone): enable this if shutdown of ros2_control_node does not fail anymore
# @launch_testing.post_shutdown_test()
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,7 @@
#include <vector>

#include "controller_interface/chainable_controller_interface.hpp"
#include "realtime_tools/realtime_buffer.h"
#include "realtime_tools/realtime_buffer.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"
// auto-generated by generate_parameter_library
#include "passthrough_controller_parameters.hpp"
Expand Down
2 changes: 1 addition & 1 deletion example_13/bringup/launch/three_robots.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -88,7 +88,7 @@ def generate_launch_description():
control_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[robot_description, robot_controllers],
parameters=[robot_controllers],
)
robot_state_pub_node = Node(
package="robot_state_publisher",
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@
#include "rclcpp/clock.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/time.hpp"
#include "realtime_tools/realtime_buffer.h"
#include "realtime_tools/realtime_buffer.hpp"

namespace ros2_control_demo_example_14
{
Expand Down
16 changes: 8 additions & 8 deletions example_15/bringup/launch/rrbot_namespace.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -69,13 +69,6 @@ def generate_launch_description():
executable="ros2_control_node",
namespace="/rrbot",
parameters=[robot_description, robot_controllers],
remappings=[
(
# we use the remapping from a relative name to FQN /position_commands
"forward_position_controller/commands",
"/position_commands",
),
],
output={
"stdout": "screen",
"stderr": "screen",
Expand Down Expand Up @@ -108,7 +101,14 @@ def generate_launch_description():
package="controller_manager",
executable="spawner",
namespace="rrbot",
arguments=["forward_position_controller", "--param-file", robot_controllers],
arguments=[
"forward_position_controller",
"--param-file",
robot_controllers,
# we use the remapping from a relative name to FQN /position_commands
"--controller-ros-args",
"-r forward_position_controller/commands:=/position_commands",
],
)

robot_position_trajectory_controller_spawner = Node(
Expand Down
11 changes: 7 additions & 4 deletions example_2/bringup/launch/diffbot.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -75,9 +75,6 @@ def generate_launch_description():
executable="ros2_control_node",
parameters=[robot_controllers],
output="both",
remappings=[
("/diffbot_base_controller/cmd_vel", "/cmd_vel"),
],
)
robot_state_pub_node = Node(
package="robot_state_publisher",
Expand All @@ -103,7 +100,13 @@ def generate_launch_description():
robot_controller_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=["diffbot_base_controller", "--param-file", robot_controllers],
arguments=[
"diffbot_base_controller",
"--param-file",
robot_controllers,
"--controller-ros-args",
"-r /diffbot_base_controller/cmd_vel:=/cmd_vel",
],
)

# Delay rviz start after `joint_state_broadcaster`
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@
#include "rclcpp/timer.hpp"
#include "rclcpp_lifecycle/lifecycle_publisher.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "realtime_tools/realtime_buffer.h"
#include "realtime_tools/realtime_buffer.hpp"
#include "trajectory_msgs/msg/joint_trajectory.hpp"
#include "trajectory_msgs/msg/joint_trajectory_point.hpp"

Expand All @@ -42,43 +42,32 @@ namespace ros2_control_demo_example_7
class RobotController : public controller_interface::ControllerInterface
{
public:
CONTROLLER_INTERFACE_PUBLIC
RobotController();

CONTROLLER_INTERFACE_PUBLIC
controller_interface::InterfaceConfiguration command_interface_configuration() const override;

CONTROLLER_INTERFACE_PUBLIC
controller_interface::InterfaceConfiguration state_interface_configuration() const override;

CONTROLLER_INTERFACE_PUBLIC
controller_interface::return_type update(
const rclcpp::Time & time, const rclcpp::Duration & period) override;

CONTROLLER_INTERFACE_PUBLIC
controller_interface::CallbackReturn on_init() override;

CONTROLLER_INTERFACE_PUBLIC
controller_interface::CallbackReturn on_configure(
const rclcpp_lifecycle::State & previous_state) override;

CONTROLLER_INTERFACE_PUBLIC
controller_interface::CallbackReturn on_activate(
const rclcpp_lifecycle::State & previous_state) override;

CONTROLLER_INTERFACE_PUBLIC
controller_interface::CallbackReturn on_deactivate(
const rclcpp_lifecycle::State & previous_state) override;

CONTROLLER_INTERFACE_PUBLIC
controller_interface::CallbackReturn on_cleanup(
const rclcpp_lifecycle::State & previous_state) override;

CONTROLLER_INTERFACE_PUBLIC
controller_interface::CallbackReturn on_error(
const rclcpp_lifecycle::State & previous_state) override;

CONTROLLER_INTERFACE_PUBLIC
controller_interface::CallbackReturn on_shutdown(
const rclcpp_lifecycle::State & previous_state) override;

Expand Down
2 changes: 1 addition & 1 deletion example_7/doc/userdoc.rst
Original file line number Diff line number Diff line change
Expand Up @@ -207,7 +207,7 @@ There are more methods that can be implemented for lifecycle changes, but they a
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
#include "hardware_interface/types/hardware_interface_return_values.hpp"

class HARDWARE_INTERFACE_PUBLIC RobotSystem : public hardware_interface::SystemInterface {
class RobotSystem : public hardware_interface::SystemInterface {
public:
CallbackReturn on_init(const hardware_interface::HardwareInfo &info) override;
CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override;
Expand Down

0 comments on commit f877a00

Please sign in to comment.