Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix tests for using new get_node_options API #840

Merged
Merged
Show file tree
Hide file tree
Changes from 3 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
7 changes: 6 additions & 1 deletion admittance_controller/test/test_admittance_controller.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -122,11 +122,16 @@ class TestableAdmittanceController : public admittance_controller::AdmittanceCon
return success;
}

rclcpp::NodeOptions get_node_options() const override { return node_options_; }

void set_node_options(const rclcpp::NodeOptions & node_options) { node_options_ = node_options; }

private:
rclcpp::WaitSet input_wrench_command_subscriber_wait_set_;
rclcpp::WaitSet input_pose_command_subscriber_wait_set_;
const std::string robot_description_ = ros2_control_test_assets::valid_6d_robot_urdf;
const std::string robot_description_semantic_ = ros2_control_test_assets::valid_6d_robot_srdf;
rclcpp::NodeOptions node_options_;
};

class AdmittanceControllerTest : public ::testing::Test
Expand Down Expand Up @@ -185,7 +190,7 @@ class AdmittanceControllerTest : public ::testing::Test
controller_interface::return_type SetUpControllerCommon(
const std::string & controller_name, const rclcpp::NodeOptions & options)
{
auto result = controller_->init(controller_name, "", 0, "", options);
auto result = controller_->init(controller_name, "", 0, "");

controller_->export_reference_interfaces();
assign_interfaces();
Expand Down
4 changes: 3 additions & 1 deletion doc/writing_new_controller.rst
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,9 @@ The following is a step-by-step guide to create source files, basic tests, and c
5. Add a constructor without parameters and the following public methods overriding the ``ControllerInterface`` definition: ``on_init``, ``command_interface_configuration``, ``state_interface_configuration``, ``on_configure``, ``on_activate``, ``on_deactivate``, ``update``.
For exact definitions check the ``controller_interface/controller_interface.hpp`` header or one of the controllers from `ros2_controllers <https://github.com/ros-controls/ros2_controllers>`_.

6. (optional) Often, controllers accept lists of joint names and interface names as parameters.
6. (Optional) The NodeOptions of the LifecycleNode can be personalized by overriding the default method ``get_node_options``.

7. (Optional) Often, controllers accept lists of joint names and interface names as parameters.
If so, you can add two protected string vectors to store those values.

4. **Adding definitions into source file (.cpp)**
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -66,6 +66,8 @@ class TestableJointTrajectoryController
return ret;
}

rclcpp::NodeOptions get_node_options() const override { return node_options_; }

/**
* @brief wait_for_trajectory block until a new JointTrajectory is received.
* Requires that the executor is not spinned elsewhere between the
Expand Down Expand Up @@ -157,6 +159,8 @@ class TestableJointTrajectoryController

double get_cmd_timeout() { return cmd_timeout_; }

void set_node_options(const rclcpp::NodeOptions & node_options) { node_options_ = node_options; }

trajectory_msgs::msg::JointTrajectoryPoint get_state_feedback() { return state_current_; }
trajectory_msgs::msg::JointTrajectoryPoint get_state_reference() { return state_desired_; }
trajectory_msgs::msg::JointTrajectoryPoint get_state_error() { return state_error_; }
Expand All @@ -179,6 +183,7 @@ class TestableJointTrajectoryController
}

rclcpp::WaitSet joint_cmd_sub_wait_set_;
rclcpp::NodeOptions node_options_;
};

class TrajectoryControllerTest : public ::testing::Test
Expand Down Expand Up @@ -233,8 +238,9 @@ class TrajectoryControllerTest : public ::testing::Test
parameter_overrides.push_back(rclcpp::Parameter("state_interfaces", state_interface_types_));
parameter_overrides.insert(parameter_overrides.end(), parameters.begin(), parameters.end());
node_options.parameter_overrides(parameter_overrides);
traj_controller_->set_node_options(node_options);

return traj_controller_->init(controller_name_, "", 0, "", node_options);
return traj_controller_->init(controller_name_, "", 0, "");
}

void SetPidParameters(
Expand Down
Loading