-
Notifications
You must be signed in to change notification settings - Fork 82
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Add test to check position controller (#134)
* Add test to check position controller Signed-off-by: Alejandro Hernández Cordero <ahcorde@gmail.com> * make linters happy Signed-off-by: Alejandro Hernández Cordero <ahcorde@gmail.com> --------- Signed-off-by: Alejandro Hernández Cordero <ahcorde@gmail.com>
- Loading branch information
Showing
7 changed files
with
544 additions
and
0 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,38 @@ | ||
cmake_minimum_required(VERSION 3.5) | ||
project(gz_ros2_control_tests) | ||
|
||
find_package(ament_cmake REQUIRED) | ||
|
||
if(BUILD_TESTING) | ||
find_package(ament_lint_auto REQUIRED) | ||
ament_lint_auto_find_test_dependencies() | ||
|
||
find_package(control_msgs REQUIRED) | ||
find_package(rclcpp REQUIRED) | ||
find_package(rclcpp_action REQUIRED) | ||
|
||
add_executable(test_position src/test_position.cpp) | ||
ament_target_dependencies(test_position | ||
rclcpp | ||
rclcpp_action | ||
control_msgs | ||
) | ||
|
||
## Install | ||
install( | ||
TARGETS | ||
test_position | ||
DESTINATION | ||
lib/${PROJECT_NAME} | ||
) | ||
|
||
install(DIRECTORY | ||
urdf | ||
config | ||
DESTINATION share/${PROJECT_NAME}/ | ||
) | ||
|
||
add_subdirectory(tests) | ||
endif() | ||
|
||
ament_package() |
19 changes: 19 additions & 0 deletions
19
gz_ros2_control_tests/config/cartpole_controller_position.yaml
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,19 @@ | ||
controller_manager: | ||
ros__parameters: | ||
update_rate: 1000 # Hz | ||
|
||
joint_trajectory_controller: | ||
type: joint_trajectory_controller/JointTrajectoryController | ||
|
||
joint_state_broadcaster: | ||
type: joint_state_broadcaster/JointStateBroadcaster | ||
|
||
joint_trajectory_controller: | ||
ros__parameters: | ||
joints: | ||
- slider_to_cart | ||
command_interfaces: | ||
- position | ||
state_interfaces: | ||
- position | ||
- velocity |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,41 @@ | ||
<?xml version="1.0"?> | ||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> | ||
<package format="3"> | ||
<name>gz_ros2_control_tests</name> | ||
<version>0.4.3</version> | ||
<description>Gazebo ros2 control tests</description> | ||
<maintainer email="alejandro@openrobotics.org">Alejandro Hernández</maintainer> | ||
<license>Apache License 2.0</license> | ||
|
||
<buildtool_depend>ament_cmake</buildtool_depend> | ||
|
||
<build_depend>control_msgs</build_depend> | ||
<build_depend>rclcpp</build_depend> | ||
<build_depend>rclcpp_action</build_depend> | ||
|
||
<exec_depend>ament_index_python</exec_depend> | ||
<exec_depend>control_msgs</exec_depend> | ||
<exec_depend>geometry_msgs</exec_depend> | ||
<exec_depend>hardware_interface</exec_depend> | ||
<exec_depend>gz_ros2_control</exec_depend> | ||
<exec_depend>joint_state_broadcaster</exec_depend> | ||
<exec_depend>joint_trajectory_controller</exec_depend> | ||
<exec_depend>launch</exec_depend> | ||
<exec_depend>launch_ros</exec_depend> | ||
<exec_depend>ros2launch</exec_depend> | ||
<exec_depend>rclcpp</exec_depend> | ||
<exec_depend>robot_state_publisher</exec_depend> | ||
<exec_depend condition="$GZ_VERSION == ''">ros_gz_sim</exec_depend> | ||
<exec_depend condition="$GZ_VERSION == 'garden'">ros_gz_sim</exec_depend> | ||
<exec_depend condition="$GZ_VERSION == fortress">ros_ign_gazebo</exec_depend> | ||
<exec_depend>ros2controlcli</exec_depend> | ||
<exec_depend>xacro</exec_depend> | ||
|
||
<test_depend>ament_cmake_gtest</test_depend> | ||
<test_depend>ament_lint_auto</test_depend> | ||
<test_depend>ament_lint_common</test_depend> | ||
|
||
<export> | ||
<build_type>ament_cmake</build_type> | ||
</export> | ||
</package> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,196 @@ | ||
// Copyright 2021 Open Source Robotics Foundation, Inc. | ||
// | ||
// 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 <memory> | ||
#include <string> | ||
#include <vector> | ||
|
||
#include <rclcpp/rclcpp.hpp> | ||
#include <rclcpp_action/rclcpp_action.hpp> | ||
|
||
#include <control_msgs/action/follow_joint_trajectory.hpp> | ||
|
||
std::shared_ptr<rclcpp::Node> node; | ||
bool common_goal_accepted = false; | ||
rclcpp_action::ResultCode common_resultcode = rclcpp_action::ResultCode::UNKNOWN; | ||
int common_action_result_code = control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL; | ||
|
||
std::vector<double> derired_goals = {0, -1, 1, 0.0}; | ||
unsigned int goal_reached = 0; | ||
|
||
void common_goal_response( | ||
rclcpp_action::ClientGoalHandle | ||
<control_msgs::action::FollowJointTrajectory>::SharedPtr future) | ||
{ | ||
RCLCPP_DEBUG( | ||
node->get_logger(), "common_goal_response time: %f", | ||
rclcpp::Clock().now().seconds()); | ||
auto goal_handle = future.get(); | ||
if (!goal_handle) { | ||
common_goal_accepted = false; | ||
printf("Goal rejected\n"); | ||
} else { | ||
common_goal_accepted = true; | ||
printf("Goal accepted\n"); | ||
} | ||
} | ||
|
||
void common_result_response( | ||
const rclcpp_action::ClientGoalHandle | ||
<control_msgs::action::FollowJointTrajectory>::WrappedResult & result) | ||
{ | ||
printf("common_result_response time: %f\n", rclcpp::Clock().now().seconds()); | ||
common_resultcode = result.code; | ||
common_action_result_code = result.result->error_code; | ||
switch (result.code) { | ||
case rclcpp_action::ResultCode::SUCCEEDED: | ||
printf("SUCCEEDED result code\n"); | ||
break; | ||
case rclcpp_action::ResultCode::ABORTED: | ||
printf("Goal was aborted\n"); | ||
return; | ||
case rclcpp_action::ResultCode::CANCELED: | ||
printf("Goal was canceled\n"); | ||
return; | ||
default: | ||
printf("Unknown result code\n"); | ||
return; | ||
} | ||
} | ||
|
||
void common_feedback( | ||
rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>::SharedPtr, | ||
const std::shared_ptr<const control_msgs::action::FollowJointTrajectory::Feedback> feedback) | ||
{ | ||
std::cout << "feedback->desired.positions :"; | ||
for (auto & x : feedback->actual.positions) { | ||
std::cout << x << "\t"; | ||
} | ||
|
||
std::cout << std::endl; | ||
if (fabs(feedback->desired.positions[0] - derired_goals[goal_reached]) < 0.1) { | ||
std::cout << "Goal " << derired_goals[goal_reached] << " reached" << '\n'; | ||
goal_reached++; | ||
std::cout << "next goal " << goal_reached << '\n'; | ||
} | ||
} | ||
|
||
int main(int argc, char * argv[]) | ||
{ | ||
rclcpp::init(argc, argv); | ||
|
||
node = std::make_shared<rclcpp::Node>("trajectory_test_node"); | ||
|
||
std::cout << "node created" << std::endl; | ||
|
||
rclcpp_action::Client<control_msgs::action::FollowJointTrajectory>::SharedPtr action_client; | ||
action_client = rclcpp_action::create_client<control_msgs::action::FollowJointTrajectory>( | ||
node->get_node_base_interface(), | ||
node->get_node_graph_interface(), | ||
node->get_node_logging_interface(), | ||
node->get_node_waitables_interface(), | ||
"/joint_trajectory_controller/follow_joint_trajectory"); | ||
|
||
bool response = | ||
action_client->wait_for_action_server(std::chrono::seconds(1)); | ||
if (!response) { | ||
throw std::runtime_error("could not get action server"); | ||
} | ||
std::cout << "Created action server" << std::endl; | ||
|
||
std::vector<std::string> joint_names = {"slider_to_cart"}; | ||
|
||
std::vector<trajectory_msgs::msg::JointTrajectoryPoint> points; | ||
trajectory_msgs::msg::JointTrajectoryPoint point; | ||
point.time_from_start = rclcpp::Duration::from_seconds(0.0); // start asap | ||
point.positions.resize(joint_names.size()); | ||
|
||
point.positions[0] = 0.0; | ||
|
||
trajectory_msgs::msg::JointTrajectoryPoint point2; | ||
point2.time_from_start = rclcpp::Duration::from_seconds(1.0); | ||
point2.positions.resize(joint_names.size()); | ||
point2.positions[0] = -1.0; | ||
|
||
trajectory_msgs::msg::JointTrajectoryPoint point3; | ||
point3.time_from_start = rclcpp::Duration::from_seconds(2.0); | ||
point3.positions.resize(joint_names.size()); | ||
point3.positions[0] = 1.0; | ||
|
||
trajectory_msgs::msg::JointTrajectoryPoint point4; | ||
point4.time_from_start = rclcpp::Duration::from_seconds(3.0); | ||
point4.positions.resize(joint_names.size()); | ||
point4.positions[0] = 0.0; | ||
|
||
// points.push_back(point); | ||
points.push_back(point2); | ||
points.push_back(point3); | ||
points.push_back(point4); | ||
|
||
rclcpp_action::Client<control_msgs::action::FollowJointTrajectory>::SendGoalOptions opt; | ||
opt.goal_response_callback = std::bind(common_goal_response, std::placeholders::_1); | ||
opt.result_callback = std::bind(common_result_response, std::placeholders::_1); | ||
opt.feedback_callback = std::bind(common_feedback, std::placeholders::_1, std::placeholders::_2); | ||
|
||
control_msgs::action::FollowJointTrajectory_Goal goal_msg; | ||
goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(1.0); | ||
goal_msg.trajectory.joint_names = joint_names; | ||
goal_msg.trajectory.points = points; | ||
|
||
auto goal_handle_future = action_client->async_send_goal(goal_msg, opt); | ||
|
||
if (rclcpp::spin_until_future_complete(node, goal_handle_future) != | ||
rclcpp::FutureReturnCode::SUCCESS) | ||
{ | ||
RCLCPP_ERROR(node->get_logger(), "send goal call failed :("); | ||
action_client.reset(); | ||
node.reset(); | ||
return 1; | ||
} | ||
RCLCPP_INFO(node->get_logger(), "send goal call ok :)"); | ||
|
||
rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>::SharedPtr | ||
goal_handle = goal_handle_future.get(); | ||
if (!goal_handle) { | ||
RCLCPP_ERROR(node->get_logger(), "Goal was rejected by server"); | ||
action_client.reset(); | ||
node.reset(); | ||
return 1; | ||
} | ||
RCLCPP_INFO(node->get_logger(), "Goal was accepted by server"); | ||
|
||
// Wait for the server to be done with the goal | ||
auto result_future = action_client->async_get_result(goal_handle); | ||
RCLCPP_INFO(node->get_logger(), "Waiting for result"); | ||
if (rclcpp::spin_until_future_complete(node, result_future) != | ||
rclcpp::FutureReturnCode::SUCCESS) | ||
{ | ||
RCLCPP_ERROR(node->get_logger(), "get result call failed :("); | ||
action_client.reset(); | ||
node.reset(); | ||
return 1; | ||
} | ||
|
||
action_client.reset(); | ||
node.reset(); | ||
|
||
std::cout << "async_send_goal" << std::endl; | ||
rclcpp::shutdown(); | ||
|
||
if ((derired_goals.size() + 1) != goal_reached) { | ||
return -1; | ||
} | ||
|
||
return 0; | ||
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,6 @@ | ||
find_package(launch_testing_ament_cmake REQUIRED) | ||
|
||
add_launch_test(position_test.py | ||
TARGET test_position_controller | ||
TIMEOUT 120 | ||
) |
Oops, something went wrong.