From 189fb804a66a124cedded096e3c980b12f0f43b6 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 28 May 2020 10:25:12 +0200 Subject: [PATCH 01/14] Added initial demos in gazebo_ros2_control_demos Signed-off-by: ahcorde --- gazebo_ros2_control_demos/CMakeLists.txt | 37 ++++ .../config/cartpole_controller.yaml | 20 ++ .../examples/example_position.cpp | 179 ++++++++++++++++++ .../examples/example_velocity.cpp | 179 ++++++++++++++++++ .../launch/cart_example_position.launch.py | 64 +++++++ .../launch/cart_example_velocity.launch.py | 64 +++++++ gazebo_ros2_control_demos/package.xml | 27 +++ .../urdf/test_cart_position.xacro.urdf | 69 +++++++ .../urdf/test_cart_velocity.xacro.urdf | 69 +++++++ 9 files changed, 708 insertions(+) create mode 100644 gazebo_ros2_control_demos/CMakeLists.txt create mode 100644 gazebo_ros2_control_demos/config/cartpole_controller.yaml create mode 100644 gazebo_ros2_control_demos/examples/example_position.cpp create mode 100644 gazebo_ros2_control_demos/examples/example_velocity.cpp create mode 100644 gazebo_ros2_control_demos/launch/cart_example_position.launch.py create mode 100644 gazebo_ros2_control_demos/launch/cart_example_velocity.launch.py create mode 100644 gazebo_ros2_control_demos/package.xml create mode 100644 gazebo_ros2_control_demos/urdf/test_cart_position.xacro.urdf create mode 100644 gazebo_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf diff --git a/gazebo_ros2_control_demos/CMakeLists.txt b/gazebo_ros2_control_demos/CMakeLists.txt new file mode 100644 index 00000000..4463dbf0 --- /dev/null +++ b/gazebo_ros2_control_demos/CMakeLists.txt @@ -0,0 +1,37 @@ +cmake_minimum_required(VERSION 3.5.0) +project(gazebo_ros2_control_demos) + +find_package(ament_cmake REQUIRED) +find_package(control_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) + +install(DIRECTORY + launch + config + urdf + DESTINATION share/${PROJECT_NAME}/ +) + +add_executable(example_position examples/example_position.cpp) +ament_target_dependencies(example_position + rclcpp + rclcpp_action + control_msgs +) + +add_executable(example_velocity examples/example_velocity.cpp) +ament_target_dependencies(example_velocity + rclcpp + rclcpp_action + control_msgs +) + +## Install +install(TARGETS example_position + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +ament_package() diff --git a/gazebo_ros2_control_demos/config/cartpole_controller.yaml b/gazebo_ros2_control_demos/config/cartpole_controller.yaml new file mode 100644 index 00000000..1d95260e --- /dev/null +++ b/gazebo_ros2_control_demos/config/cartpole_controller.yaml @@ -0,0 +1,20 @@ +cart_pole_controller: + ros__parameters: + type: joint_trajectory_controller/JointTrajectoryController + joints: + - slider_to_cart + write_op_modes: + - slider_to_cart + gains: + cart_to_pole: {p: 0.0, d: 0, i: 0, i_clamp: 0} + stop_trajectory_duration: 0.5 + state_publish_rate: 25 + action_monitor_rate: 20 + constraints: + stopped_velocity_tolerance: 0.05 + goal_time: 5 + +joint_state_controller: + ros__parameters: + type: joint_state_controller/JointStateController + publish_rate: 50 diff --git a/gazebo_ros2_control_demos/examples/example_position.cpp b/gazebo_ros2_control_demos/examples/example_position.cpp new file mode 100644 index 00000000..52dbcd91 --- /dev/null +++ b/gazebo_ros2_control_demos/examples/example_position.cpp @@ -0,0 +1,179 @@ +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" + +#include "control_msgs/action/follow_joint_trajectory.hpp" + +std::shared_ptr 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; + +void common_goal_response(std::shared_future::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::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::SharedPtr, + const std::shared_ptr feedback) +{ + std::cout << "feedback->desired.positions :"; + for (auto & x: feedback->desired.positions) + std::cout << x << "\t"; + std::cout << std::endl; + std::cout << "feedback->desired.velocities :"; + for (auto & x: feedback->desired.velocities) + std::cout << x << "\t"; + // for (auto & x: feedback->desired.error) + // std::cout << x << "\t"; + // for (auto & x: feedback->desired.velocities) + // std::cout << x << "\t"; + std::cout << std::endl; +} + +int main(int argc, char* argv[]) +{ + + rclcpp::init(argc, argv); + + node = std::make_shared("trajectory_test_node"); + + std::cout << "node created" << std::endl; + + rclcpp_action::Client::SharedPtr action_client; + action_client = rclcpp_action::create_client( + node->get_node_base_interface(), + node->get_node_graph_interface(), + node->get_node_logging_interface(), + node->get_node_waitables_interface(), + "/cart_pole_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 joint_names = {"slider_to_cart"}; + + std::vector 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.velocities.resize(joint_names.size()); + + point.positions[0] = 0.0; + // point.velocities[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.velocities.resize(joint_names.size()); + point2.positions[0] = -1.0; + // point2.velocities[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.velocities.resize(joint_names.size()); + point3.positions[0] = 1.0; + // point3.velocities[0] = 0.0; + + trajectory_msgs::msg::JointTrajectoryPoint point4; + point4.time_from_start = rclcpp::Duration::from_seconds(3.0); + point4.positions.resize(joint_names.size()); + // point4.velocities.resize(joint_names.size()); + point4.positions[0] = 0.0; + // point4.velocities[0] = 0.0; + + points.push_back(point); + points.push_back(point2); + points.push_back(point3); + points.push_back(point4); + + rclcpp_action::Client::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); + // [&]( + // rclcpp_action::ClientGoalHandle::SharedPtr, + // const std::shared_ptr feedback) + // { + // std::cout << "feedback->desired :"; + // for (auto & x: feedback->desired.positions) + // std::cout << x << "\t"; + // std::cout << std::endl; + // }; + + 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 :("); + return 1; + } + RCLCPP_ERROR(node->get_logger(), "send goal call ok :)"); + + rclcpp_action::ClientGoalHandle::SharedPtr goal_handle = goal_handle_future.get(); + if (!goal_handle) { + RCLCPP_ERROR(node->get_logger(), "Goal was rejected by server"); + return 1; + } + RCLCPP_ERROR(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 :("); + return 1; + } + + // while(true){} + + std::cout << "async_send_goal" << std::endl; + rclcpp::shutdown(); + + return 0; +} diff --git a/gazebo_ros2_control_demos/examples/example_velocity.cpp b/gazebo_ros2_control_demos/examples/example_velocity.cpp new file mode 100644 index 00000000..68280aa5 --- /dev/null +++ b/gazebo_ros2_control_demos/examples/example_velocity.cpp @@ -0,0 +1,179 @@ +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" + +#include "control_msgs/action/follow_joint_trajectory.hpp" + +std::shared_ptr 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; + +void common_goal_response(std::shared_future::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::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::SharedPtr, + const std::shared_ptr feedback) +{ + std::cout << "feedback->desired.positions :"; + for (auto & x: feedback->desired.positions) + std::cout << x << "\t"; + std::cout << std::endl; + std::cout << "feedback->desired.velocities :"; + for (auto & x: feedback->desired.velocities) + std::cout << x << "\t"; + std::cout << std::endl; + // std::cout << "feedback->desired.error :"; + // for (auto & x: feedback->desired.error) + // std::cout << x << "\t"; + std::cout << std::endl; +} + +int main(int argc, char* argv[]) +{ + + rclcpp::init(argc, argv); + + node = std::make_shared("trajectory_test_node"); + + std::cout << "node created" << std::endl; + + rclcpp_action::Client::SharedPtr action_client; + action_client = rclcpp_action::create_client( + node->get_node_base_interface(), + node->get_node_graph_interface(), + node->get_node_logging_interface(), + node->get_node_waitables_interface(), + "/cart_pole_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 joint_names = {"slider_to_cart"}; + + std::vector 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.velocities.resize(joint_names.size()); + + point.positions[0] = 0.0; + point.velocities[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.velocities.resize(joint_names.size()); + point2.positions[0] = -1.0; + point2.velocities[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.velocities.resize(joint_names.size()); + point3.positions[0] = 1.0; + point3.velocities[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.velocities.resize(joint_names.size()); + point4.positions[0] = 0.0; + point4.velocities[0] = 0.0; + + points.push_back(point); + points.push_back(point2); + points.push_back(point3); + points.push_back(point4); + + rclcpp_action::Client::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); + // [&]( + // rclcpp_action::ClientGoalHandle::SharedPtr, + // const std::shared_ptr feedback) + // { + // std::cout << "feedback->desired :"; + // for (auto & x: feedback->desired.positions) + // std::cout << x << "\t"; + // std::cout << std::endl; + // }; + + 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 :("); + return 1; + } + RCLCPP_ERROR(node->get_logger(), "send goal call ok :)"); + + rclcpp_action::ClientGoalHandle::SharedPtr goal_handle = goal_handle_future.get(); + if (!goal_handle) { + RCLCPP_ERROR(node->get_logger(), "Goal was rejected by server"); + return 1; + } + RCLCPP_ERROR(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 :("); + return 1; + } + + // while(true){} + + std::cout << "async_send_goal" << std::endl; + rclcpp::shutdown(); + + return 0; +} diff --git a/gazebo_ros2_control_demos/launch/cart_example_position.launch.py b/gazebo_ros2_control_demos/launch/cart_example_position.launch.py new file mode 100644 index 00000000..9cc87a00 --- /dev/null +++ b/gazebo_ros2_control_demos/launch/cart_example_position.launch.py @@ -0,0 +1,64 @@ +# Copyright 2020 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. + +import os + +from ament_index_python.packages import get_package_share_directory + + +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_context import LaunchContext +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import Command + +from launch_ros.actions import Node + + +def generate_launch_description(): + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py']), + ) + + gazebo_ros2_control_demos_path = os.path.join( + get_package_share_directory('gazebo_ros2_control_demos')) + + xacro_file = os.path.join(gazebo_ros2_control_demos_path, 'urdf', 'test_cart_position.xacro.urdf') + config_file = os.path.join(gazebo_ros2_control_demos_path, 'config', 'cartpole_controller.yaml') + + robot_desc = Command('xacro %s' % xacro_file) + params = {'robot_description': robot_desc} + + context = LaunchContext() + command = Command('xacro %s -o /tmp/test_cart_position.urdf' % xacro_file) + command.perform(context) + + node_robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='screen', + parameters=[params] + ) + + # GAZEBO_MODEL_PATH has to be correctly set for Gazebo to be able to find the model + spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py', + arguments=['-file', '/tmp/test_cart_position.urdf', '-entity', 'cartpole'], + output='screen') + + return LaunchDescription([ + gazebo, + node_robot_state_publisher, + spawn_entity, + ]) diff --git a/gazebo_ros2_control_demos/launch/cart_example_velocity.launch.py b/gazebo_ros2_control_demos/launch/cart_example_velocity.launch.py new file mode 100644 index 00000000..bfd49711 --- /dev/null +++ b/gazebo_ros2_control_demos/launch/cart_example_velocity.launch.py @@ -0,0 +1,64 @@ +# Copyright 2020 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. + +import os + +from ament_index_python.packages import get_package_share_directory + + +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_context import LaunchContext +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import Command + +from launch_ros.actions import Node + + +def generate_launch_description(): + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py']), + ) + + gazebo_ros2_control_demos_path = os.path.join( + get_package_share_directory('gazebo_ros2_control_demos')) + + xacro_file = os.path.join(gazebo_ros2_control_demos_path, 'urdf', 'test_cart_velocity.xacro.urdf') + config_file = os.path.join(gazebo_ros2_control_demos_path, 'config', 'cartpole_controller.yaml') + + robot_desc = Command('xacro %s' % xacro_file) + params = {'robot_description': robot_desc} + + context = LaunchContext() + command = Command('xacro %s -o /tmp/test_cart_velocity.urdf' % xacro_file) + command.perform(context) + + node_robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='screen', + parameters=[params] + ) + + # GAZEBO_MODEL_PATH has to be correctly set for Gazebo to be able to find the model + spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py', + arguments=['-file', '/tmp/test_cart_velocity.urdf', '-entity', 'cartpole'], + output='screen') + + return LaunchDescription([ + gazebo, + node_robot_state_publisher, + spawn_entity, + ]) diff --git a/gazebo_ros2_control_demos/package.xml b/gazebo_ros2_control_demos/package.xml new file mode 100644 index 00000000..788bdb5d --- /dev/null +++ b/gazebo_ros2_control_demos/package.xml @@ -0,0 +1,27 @@ + + + gazebo_ros2_control_demos + 0.0.1 + gazebo_ros2_control_demos + + Alejandro Hernandez + + BSD + + http://ros.org/wiki/gazebo_ros_control + https://github.com/ros-simulation/gazebo_ros2_control/issues + https://github.com/ros-simulation/gazebo_ros2_control + + Alejandro Hernández + + ament_cmake + + ament_index_python + gazebo_ros2_control + launch + launch_ros + + + ament_cmake + + diff --git a/gazebo_ros2_control_demos/urdf/test_cart_position.xacro.urdf b/gazebo_ros2_control_demos/urdf/test_cart_position.xacro.urdf new file mode 100644 index 00000000..f95badff --- /dev/null +++ b/gazebo_ros2_control_demos/urdf/test_cart_position.xacro.urdf @@ -0,0 +1,69 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 1 + + + + + / + gazebo_ros2_control/DefaultRobotHWSim + True + $(find gazebo_ros2_control_demos)/config/cartpole_controller.yaml + + + diff --git a/gazebo_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf b/gazebo_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf new file mode 100644 index 00000000..9b7e07d7 --- /dev/null +++ b/gazebo_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf @@ -0,0 +1,69 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/VelocityJointInterface + + + hardware_interface/VelocityJointInterface + 1 + + + + + / + gazebo_ros2_control/DefaultRobotHWSim + True + $(find gazebo_ros2_control_demos)/config/cartpole_controller.yaml + + + From 9eeeca4e272afb8a0a4cf1c34b1b7e8004364c82 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 28 May 2020 11:32:02 +0200 Subject: [PATCH 02/14] Fixed install in CMakeLists.txt Signed-off-by: ahcorde --- gazebo_ros2_control_demos/CMakeLists.txt | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/gazebo_ros2_control_demos/CMakeLists.txt b/gazebo_ros2_control_demos/CMakeLists.txt index 4463dbf0..475db2b6 100644 --- a/gazebo_ros2_control_demos/CMakeLists.txt +++ b/gazebo_ros2_control_demos/CMakeLists.txt @@ -28,10 +28,8 @@ ament_target_dependencies(example_velocity ) ## Install -install(TARGETS example_position - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin +install(TARGETS example_position example_velocity + DESTINATION lib/${PROJECT_NAME} ) ament_package() From 9635877111788bcb999f8c68e542b37445606486 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 28 May 2020 16:15:49 +0200 Subject: [PATCH 03/14] Removed commented code Signed-off-by: ahcorde --- .../examples/example_position.cpp | 23 ------------------- .../examples/example_velocity.cpp | 14 ----------- 2 files changed, 37 deletions(-) diff --git a/gazebo_ros2_control_demos/examples/example_position.cpp b/gazebo_ros2_control_demos/examples/example_position.cpp index 52dbcd91..9ad99201 100644 --- a/gazebo_ros2_control_demos/examples/example_position.cpp +++ b/gazebo_ros2_control_demos/examples/example_position.cpp @@ -56,10 +56,6 @@ void common_feedback(rclcpp_action::ClientGoalHandledesired.velocities :"; for (auto & x: feedback->desired.velocities) std::cout << x << "\t"; - // for (auto & x: feedback->desired.error) - // std::cout << x << "\t"; - // for (auto & x: feedback->desired.velocities) - // std::cout << x << "\t"; std::cout << std::endl; } @@ -93,31 +89,23 @@ int main(int argc, char* argv[]) trajectory_msgs::msg::JointTrajectoryPoint point; point.time_from_start = rclcpp::Duration::from_seconds(0.0); // start asap point.positions.resize(joint_names.size()); - // point.velocities.resize(joint_names.size()); point.positions[0] = 0.0; - // point.velocities[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.velocities.resize(joint_names.size()); point2.positions[0] = -1.0; - // point2.velocities[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.velocities.resize(joint_names.size()); point3.positions[0] = 1.0; - // point3.velocities[0] = 0.0; trajectory_msgs::msg::JointTrajectoryPoint point4; point4.time_from_start = rclcpp::Duration::from_seconds(3.0); point4.positions.resize(joint_names.size()); - // point4.velocities.resize(joint_names.size()); point4.positions[0] = 0.0; - // point4.velocities[0] = 0.0; points.push_back(point); points.push_back(point2); @@ -128,15 +116,6 @@ int main(int argc, char* argv[]) 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); - // [&]( - // rclcpp_action::ClientGoalHandle::SharedPtr, - // const std::shared_ptr feedback) - // { - // std::cout << "feedback->desired :"; - // for (auto & x: feedback->desired.positions) - // std::cout << x << "\t"; - // std::cout << std::endl; - // }; control_msgs::action::FollowJointTrajectory_Goal goal_msg; goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(1.0); @@ -170,8 +149,6 @@ int main(int argc, char* argv[]) return 1; } - // while(true){} - std::cout << "async_send_goal" << std::endl; rclcpp::shutdown(); diff --git a/gazebo_ros2_control_demos/examples/example_velocity.cpp b/gazebo_ros2_control_demos/examples/example_velocity.cpp index 68280aa5..158977af 100644 --- a/gazebo_ros2_control_demos/examples/example_velocity.cpp +++ b/gazebo_ros2_control_demos/examples/example_velocity.cpp @@ -57,9 +57,6 @@ void common_feedback(rclcpp_action::ClientGoalHandledesired.velocities) std::cout << x << "\t"; std::cout << std::endl; - // std::cout << "feedback->desired.error :"; - // for (auto & x: feedback->desired.error) - // std::cout << x << "\t"; std::cout << std::endl; } @@ -128,15 +125,6 @@ int main(int argc, char* argv[]) 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); - // [&]( - // rclcpp_action::ClientGoalHandle::SharedPtr, - // const std::shared_ptr feedback) - // { - // std::cout << "feedback->desired :"; - // for (auto & x: feedback->desired.positions) - // std::cout << x << "\t"; - // std::cout << std::endl; - // }; control_msgs::action::FollowJointTrajectory_Goal goal_msg; goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(1.0); @@ -170,8 +158,6 @@ int main(int argc, char* argv[]) return 1; } - // while(true){} - std::cout << "async_send_goal" << std::endl; rclcpp::shutdown(); From 961c25e27a1c19be83e5f01ee2a5d0fd86715245 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 28 May 2020 16:34:51 +0200 Subject: [PATCH 04/14] Added common ros 2 linters Signed-off-by: ahcorde --- gazebo_ros2_control_demos/CMakeLists.txt | 20 +++++++++ .../examples/example_position.cpp | 41 +++++++++++++++---- .../examples/example_velocity.cpp | 41 +++++++++++++++---- .../launch/cart_example_position.launch.py | 5 ++- .../launch/cart_example_velocity.launch.py | 6 +-- gazebo_ros2_control_demos/package.xml | 6 ++- 6 files changed, 95 insertions(+), 24 deletions(-) diff --git a/gazebo_ros2_control_demos/CMakeLists.txt b/gazebo_ros2_control_demos/CMakeLists.txt index 475db2b6..34bfa83f 100644 --- a/gazebo_ros2_control_demos/CMakeLists.txt +++ b/gazebo_ros2_control_demos/CMakeLists.txt @@ -1,6 +1,19 @@ cmake_minimum_required(VERSION 3.5.0) project(gazebo_ros2_control_demos) +# Default to C11 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 11) +endif() +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(NOT WIN32) + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + find_package(ament_cmake REQUIRED) find_package(control_msgs REQUIRED) find_package(rclcpp REQUIRED) @@ -27,6 +40,13 @@ ament_target_dependencies(example_velocity control_msgs ) +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_lint_auto REQUIRED) + + ament_lint_auto_find_test_dependencies() +endif() + ## Install install(TARGETS example_position example_velocity DESTINATION lib/${PROJECT_NAME} diff --git a/gazebo_ros2_control_demos/examples/example_position.cpp b/gazebo_ros2_control_demos/examples/example_position.cpp index 9ad99201..2cafb297 100644 --- a/gazebo_ros2_control_demos/examples/example_position.cpp +++ b/gazebo_ros2_control_demos/examples/example_position.cpp @@ -1,4 +1,20 @@ -#include +// Copyright 2020 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 +#include +#include #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" @@ -10,7 +26,9 @@ 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; -void common_goal_response(std::shared_future::SharedPtr> future) +void common_goal_response( + std::shared_future::SharedPtr> future) { RCLCPP_DEBUG( node->get_logger(), "common_goal_response time: %f", @@ -25,7 +43,9 @@ void common_goal_response(std::shared_future::WrappedResult & result) +void common_result_response( + const rclcpp_action::ClientGoalHandle + ::WrappedResult & result) { printf("common_result_response time: %f\n", rclcpp::Clock().now().seconds()); common_resultcode = result.code; @@ -46,22 +66,24 @@ void common_result_response(const rclcpp_action::ClientGoalHandle::SharedPtr, +void common_feedback( + rclcpp_action::ClientGoalHandle::SharedPtr, const std::shared_ptr feedback) { std::cout << "feedback->desired.positions :"; - for (auto & x: feedback->desired.positions) + for (auto & x : feedback->desired.positions) { std::cout << x << "\t"; + } std::cout << std::endl; std::cout << "feedback->desired.velocities :"; - for (auto & x: feedback->desired.velocities) + for (auto & x : feedback->desired.velocities) { std::cout << x << "\t"; + } std::cout << std::endl; } -int main(int argc, char* argv[]) +int main(int argc, char * argv[]) { - rclcpp::init(argc, argv); node = std::make_shared("trajectory_test_node"); @@ -132,7 +154,8 @@ int main(int argc, char* argv[]) } RCLCPP_ERROR(node->get_logger(), "send goal call ok :)"); - rclcpp_action::ClientGoalHandle::SharedPtr goal_handle = goal_handle_future.get(); + rclcpp_action::ClientGoalHandle::SharedPtr + goal_handle = goal_handle_future.get(); if (!goal_handle) { RCLCPP_ERROR(node->get_logger(), "Goal was rejected by server"); return 1; diff --git a/gazebo_ros2_control_demos/examples/example_velocity.cpp b/gazebo_ros2_control_demos/examples/example_velocity.cpp index 158977af..528af503 100644 --- a/gazebo_ros2_control_demos/examples/example_velocity.cpp +++ b/gazebo_ros2_control_demos/examples/example_velocity.cpp @@ -1,4 +1,20 @@ -#include +// Copyright 2020 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 +#include +#include #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" @@ -10,7 +26,9 @@ 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; -void common_goal_response(std::shared_future::SharedPtr> future) +void common_goal_response( + std::shared_future::SharedPtr> future) { RCLCPP_DEBUG( node->get_logger(), "common_goal_response time: %f", @@ -25,7 +43,9 @@ void common_goal_response(std::shared_future::WrappedResult & result) +void common_result_response( + const rclcpp_action::ClientGoalHandle + ::WrappedResult & result) { printf("common_result_response time: %f\n", rclcpp::Clock().now().seconds()); common_resultcode = result.code; @@ -46,23 +66,25 @@ void common_result_response(const rclcpp_action::ClientGoalHandle::SharedPtr, +void common_feedback( + rclcpp_action::ClientGoalHandle::SharedPtr, const std::shared_ptr feedback) { std::cout << "feedback->desired.positions :"; - for (auto & x: feedback->desired.positions) + for (auto & x : feedback->desired.positions) { std::cout << x << "\t"; + } std::cout << std::endl; std::cout << "feedback->desired.velocities :"; - for (auto & x: feedback->desired.velocities) + for (auto & x : feedback->desired.velocities) { std::cout << x << "\t"; + } std::cout << std::endl; std::cout << std::endl; } -int main(int argc, char* argv[]) +int main(int argc, char * argv[]) { - rclcpp::init(argc, argv); node = std::make_shared("trajectory_test_node"); @@ -141,7 +163,8 @@ int main(int argc, char* argv[]) } RCLCPP_ERROR(node->get_logger(), "send goal call ok :)"); - rclcpp_action::ClientGoalHandle::SharedPtr goal_handle = goal_handle_future.get(); + rclcpp_action::ClientGoalHandle::SharedPtr + goal_handle = goal_handle_future.get(); if (!goal_handle) { RCLCPP_ERROR(node->get_logger(), "Goal was rejected by server"); return 1; diff --git a/gazebo_ros2_control_demos/launch/cart_example_position.launch.py b/gazebo_ros2_control_demos/launch/cart_example_position.launch.py index 9cc87a00..29a9677b 100644 --- a/gazebo_ros2_control_demos/launch/cart_example_position.launch.py +++ b/gazebo_ros2_control_demos/launch/cart_example_position.launch.py @@ -35,8 +35,9 @@ def generate_launch_description(): gazebo_ros2_control_demos_path = os.path.join( get_package_share_directory('gazebo_ros2_control_demos')) - xacro_file = os.path.join(gazebo_ros2_control_demos_path, 'urdf', 'test_cart_position.xacro.urdf') - config_file = os.path.join(gazebo_ros2_control_demos_path, 'config', 'cartpole_controller.yaml') + xacro_file = os.path.join(gazebo_ros2_control_demos_path, + 'urdf', + 'test_cart_position.xacro.urdf') robot_desc = Command('xacro %s' % xacro_file) params = {'robot_description': robot_desc} diff --git a/gazebo_ros2_control_demos/launch/cart_example_velocity.launch.py b/gazebo_ros2_control_demos/launch/cart_example_velocity.launch.py index bfd49711..fa871851 100644 --- a/gazebo_ros2_control_demos/launch/cart_example_velocity.launch.py +++ b/gazebo_ros2_control_demos/launch/cart_example_velocity.launch.py @@ -35,9 +35,9 @@ def generate_launch_description(): gazebo_ros2_control_demos_path = os.path.join( get_package_share_directory('gazebo_ros2_control_demos')) - xacro_file = os.path.join(gazebo_ros2_control_demos_path, 'urdf', 'test_cart_velocity.xacro.urdf') - config_file = os.path.join(gazebo_ros2_control_demos_path, 'config', 'cartpole_controller.yaml') - + xacro_file = os.path.join(gazebo_ros2_control_demos_path, + 'urdf', + 'test_cart_velocity.xacro.urdf') robot_desc = Command('xacro %s' % xacro_file) params = {'robot_description': robot_desc} diff --git a/gazebo_ros2_control_demos/package.xml b/gazebo_ros2_control_demos/package.xml index 788bdb5d..c08998c7 100644 --- a/gazebo_ros2_control_demos/package.xml +++ b/gazebo_ros2_control_demos/package.xml @@ -6,7 +6,7 @@ Alejandro Hernandez - BSD + Apache License 2.0 http://ros.org/wiki/gazebo_ros_control https://github.com/ros-simulation/gazebo_ros2_control/issues @@ -21,6 +21,10 @@ launch launch_ros + ament_cmake_gtest + ament_lint_auto + ament_lint_common + ament_cmake From 7ac0bd708046b5b11464c3fe112d17ebd43efa1b Mon Sep 17 00:00:00 2001 From: ahcorde Date: Mon, 22 Jun 2020 09:33:31 +0200 Subject: [PATCH 05/14] Added position pid example Signed-off-by: ahcorde --- .../cart_example_position_pid.launch.py | 66 ++++++++++++++++ .../urdf/test_cart_position_pid.xacro.urdf | 77 +++++++++++++++++++ 2 files changed, 143 insertions(+) create mode 100644 gazebo_ros2_control_demos/launch/cart_example_position_pid.launch.py create mode 100644 gazebo_ros2_control_demos/urdf/test_cart_position_pid.xacro.urdf diff --git a/gazebo_ros2_control_demos/launch/cart_example_position_pid.launch.py b/gazebo_ros2_control_demos/launch/cart_example_position_pid.launch.py new file mode 100644 index 00000000..cf4f9f18 --- /dev/null +++ b/gazebo_ros2_control_demos/launch/cart_example_position_pid.launch.py @@ -0,0 +1,66 @@ +# Copyright 2020 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. + +import os + +from ament_index_python.packages import get_package_share_directory + + +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_context import LaunchContext +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import Command + +from launch_ros.actions import Node + + +def generate_launch_description(): + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py']), + ) + + gazebo_ros2_control_demos_path = os.path.join( + get_package_share_directory('gazebo_ros2_control_demos')) + + xacro_file = os.path.join(gazebo_ros2_control_demos_path, + 'urdf', + 'test_cart_position_pid.xacro.urdf') + + robot_desc = Command('xacro %s' % xacro_file) + params = {'robot_description': robot_desc} + + context = LaunchContext() + command = Command('xacro %s -o /tmp/test_cart_position_pid.urdf' % xacro_file) + command.perform(context) + + node_robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='screen', + parameters=[params] + ) + + # GAZEBO_MODEL_PATH has to be correctly set for Gazebo to be able to find the model + spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py', + arguments=['-file', '/tmp/test_cart_position_pid.urdf', + '-entity', 'cartpole'], + output='screen') + + return LaunchDescription([ + gazebo, + node_robot_state_publisher, + spawn_entity, + ]) diff --git a/gazebo_ros2_control_demos/urdf/test_cart_position_pid.xacro.urdf b/gazebo_ros2_control_demos/urdf/test_cart_position_pid.xacro.urdf new file mode 100644 index 00000000..09728a64 --- /dev/null +++ b/gazebo_ros2_control_demos/urdf/test_cart_position_pid.xacro.urdf @@ -0,0 +1,77 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 1 + + + + + + / + 50.0 + 10.0 + 15.0 + 3.0 + -3.0 + false + + gazebo_ros2_control/DefaultRobotHWSim + True + $(find gazebo_ros2_control_demos)/config/cartpole_controller.yaml + + + From a54fc2fc2f561fed671c108a1d6851406f0cb4a8 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Mon, 22 Jun 2020 10:47:05 +0200 Subject: [PATCH 06/14] clean config file Signed-off-by: ahcorde --- gazebo_ros2_control_demos/config/cartpole_controller.yaml | 3 --- 1 file changed, 3 deletions(-) diff --git a/gazebo_ros2_control_demos/config/cartpole_controller.yaml b/gazebo_ros2_control_demos/config/cartpole_controller.yaml index 1d95260e..1f39ba1e 100644 --- a/gazebo_ros2_control_demos/config/cartpole_controller.yaml +++ b/gazebo_ros2_control_demos/config/cartpole_controller.yaml @@ -5,9 +5,6 @@ cart_pole_controller: - slider_to_cart write_op_modes: - slider_to_cart - gains: - cart_to_pole: {p: 0.0, d: 0, i: 0, i_clamp: 0} - stop_trajectory_duration: 0.5 state_publish_rate: 25 action_monitor_rate: 20 constraints: From 28722d210e27153ffca37d3bfd584427adae2652 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 25 Jun 2020 08:50:43 +0200 Subject: [PATCH 07/14] Included test in gazebo_ros2_control_demos Signed-off-by: ahcorde --- .github/workflows/ci.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 5da42f94..7a4e4a17 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -1,4 +1,4 @@ -name: Gazebo ros2 control CI +cdname: Gazebo ros2 control CI on: pull_request: @@ -55,5 +55,5 @@ jobs: run: | cd /home/ros2_ws/ . /opt/ros/foxy/local_setup.sh - colcon test --event-handlers console_direct+ --packages-select gazebo_ros2_control + colcon test --event-handlers console_direct+ --packages-select gazebo_ros2_control gazebo_ros2_control_demos colcon test-result From 48fb15aa5468d8625034f11432bcd5544ef4f0fc Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 25 Jun 2020 08:51:00 +0200 Subject: [PATCH 08/14] Added gazebo_ros as exec depend Signed-off-by: ahcorde --- gazebo_ros2_control_demos/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/gazebo_ros2_control_demos/package.xml b/gazebo_ros2_control_demos/package.xml index c08998c7..388cadc4 100644 --- a/gazebo_ros2_control_demos/package.xml +++ b/gazebo_ros2_control_demos/package.xml @@ -18,6 +18,7 @@ ament_index_python gazebo_ros2_control + gazebo_ros launch launch_ros From 5b444fed05eb911ec77f08c831ff4b88d9035653 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 25 Jun 2020 08:53:09 +0200 Subject: [PATCH 09/14] Fixed ci.yml Signed-off-by: ahcorde --- .github/workflows/ci.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 7a4e4a17..26f592ec 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -1,4 +1,4 @@ -cdname: Gazebo ros2 control CI +name: Gazebo ros2 control CI on: pull_request: From a37fe34d3f638557d2951fba65a6c873726f9b5d Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 26 Jun 2020 10:42:27 +0200 Subject: [PATCH 10/14] updated xacro files Signed-off-by: ahcorde --- gazebo_ros2_control_demos/urdf/test_cart_position.xacro.urdf | 4 +--- .../urdf/test_cart_position_pid.xacro.urdf | 3 +-- gazebo_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf | 4 +--- 3 files changed, 3 insertions(+), 8 deletions(-) diff --git a/gazebo_ros2_control_demos/urdf/test_cart_position.xacro.urdf b/gazebo_ros2_control_demos/urdf/test_cart_position.xacro.urdf index f95badff..2f0d8f82 100644 --- a/gazebo_ros2_control_demos/urdf/test_cart_position.xacro.urdf +++ b/gazebo_ros2_control_demos/urdf/test_cart_position.xacro.urdf @@ -60,9 +60,7 @@ - / - gazebo_ros2_control/DefaultRobotHWSim - True + gazebo_ros2_control/DefaultRobotHWSim $(find gazebo_ros2_control_demos)/config/cartpole_controller.yaml diff --git a/gazebo_ros2_control_demos/urdf/test_cart_position_pid.xacro.urdf b/gazebo_ros2_control_demos/urdf/test_cart_position_pid.xacro.urdf index 09728a64..2fed7bf0 100644 --- a/gazebo_ros2_control_demos/urdf/test_cart_position_pid.xacro.urdf +++ b/gazebo_ros2_control_demos/urdf/test_cart_position_pid.xacro.urdf @@ -69,8 +69,7 @@ -3.0 false - gazebo_ros2_control/DefaultRobotHWSim - True + gazebo_ros2_control/DefaultRobotHWSim $(find gazebo_ros2_control_demos)/config/cartpole_controller.yaml diff --git a/gazebo_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf b/gazebo_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf index 9b7e07d7..490a1f7c 100644 --- a/gazebo_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf +++ b/gazebo_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf @@ -60,9 +60,7 @@ - / - gazebo_ros2_control/DefaultRobotHWSim - True + gazebo_ros2_control/DefaultRobotHWSim $(find gazebo_ros2_control_demos)/config/cartpole_controller.yaml From 927a59bf9f16a06279795665467f702469b71763 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 23 Jul 2020 15:41:58 -0700 Subject: [PATCH 11/14] up to demos Signed-off-by: Louise Poubel --- .github/workflows/ci.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 5b913fc9..bb46e97c 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -51,7 +51,7 @@ jobs: cd /home/ros2_ws/ . /opt/ros/foxy/local_setup.sh export CMAKE_PREFIX_PATH=$AMENT_PREFIX_PATH:$CMAKE_PREFIX_PATH - colcon build --packages-up-to gazebo_ros2_control + colcon build --packages-up-to gazebo_ros2_control_demos - name: Run tests id: test run: | From 73a66bb718af0af495d3838f45a438a6fa48cdc3 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 24 Jul 2020 10:27:03 +0200 Subject: [PATCH 12/14] Included dependencies in the CMakeLists.txt Signed-off-by: ahcorde --- gazebo_ros2_control_demos/package.xml | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/gazebo_ros2_control_demos/package.xml b/gazebo_ros2_control_demos/package.xml index 388cadc4..b74c7ed1 100644 --- a/gazebo_ros2_control_demos/package.xml +++ b/gazebo_ros2_control_demos/package.xml @@ -16,11 +16,23 @@ ament_cmake + control_msgs + rclcpp + rclcpp_action + ament_index_python + control_msgs gazebo_ros2_control gazebo_ros + hardware_interface + joint_trajectory_controller + joint_state_controller launch launch_ros + robot_state_publisher + rclcpp + transmission_interface + xacro ament_cmake_gtest ament_lint_auto From a8ebd157e0ab21ce8d30592bf82ee0b90bb240bc Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 28 Jul 2020 08:38:16 +0200 Subject: [PATCH 13/14] avoid tmp files and removed comment Signed-off-by: ahcorde --- .../launch/cart_example_position.launch.py | 12 +++++------- .../launch/cart_example_position_pid.launch.py | 12 +++++------- .../launch/cart_example_velocity.launch.py | 12 +++++------- 3 files changed, 15 insertions(+), 21 deletions(-) diff --git a/gazebo_ros2_control_demos/launch/cart_example_position.launch.py b/gazebo_ros2_control_demos/launch/cart_example_position.launch.py index 29a9677b..a8f99513 100644 --- a/gazebo_ros2_control_demos/launch/cart_example_position.launch.py +++ b/gazebo_ros2_control_demos/launch/cart_example_position.launch.py @@ -25,6 +25,8 @@ from launch_ros.actions import Node +import xacro + def generate_launch_description(): gazebo = IncludeLaunchDescription( @@ -39,12 +41,9 @@ def generate_launch_description(): 'urdf', 'test_cart_position.xacro.urdf') - robot_desc = Command('xacro %s' % xacro_file) - params = {'robot_description': robot_desc} - - context = LaunchContext() - command = Command('xacro %s -o /tmp/test_cart_position.urdf' % xacro_file) - command.perform(context) + doc = xacro.parse(open(xacro_file)) + xacro.process_doc(doc) + params = {'robot_description': doc.toxml()} node_robot_state_publisher = Node( package='robot_state_publisher', @@ -53,7 +52,6 @@ def generate_launch_description(): parameters=[params] ) - # GAZEBO_MODEL_PATH has to be correctly set for Gazebo to be able to find the model spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py', arguments=['-file', '/tmp/test_cart_position.urdf', '-entity', 'cartpole'], output='screen') diff --git a/gazebo_ros2_control_demos/launch/cart_example_position_pid.launch.py b/gazebo_ros2_control_demos/launch/cart_example_position_pid.launch.py index cf4f9f18..b74e63a8 100644 --- a/gazebo_ros2_control_demos/launch/cart_example_position_pid.launch.py +++ b/gazebo_ros2_control_demos/launch/cart_example_position_pid.launch.py @@ -25,6 +25,8 @@ from launch_ros.actions import Node +import xacro + def generate_launch_description(): gazebo = IncludeLaunchDescription( @@ -39,12 +41,9 @@ def generate_launch_description(): 'urdf', 'test_cart_position_pid.xacro.urdf') - robot_desc = Command('xacro %s' % xacro_file) - params = {'robot_description': robot_desc} - - context = LaunchContext() - command = Command('xacro %s -o /tmp/test_cart_position_pid.urdf' % xacro_file) - command.perform(context) + doc = xacro.parse(open(xacro_file)) + xacro.process_doc(doc) + params = {'robot_description': doc.toxml()} node_robot_state_publisher = Node( package='robot_state_publisher', @@ -53,7 +52,6 @@ def generate_launch_description(): parameters=[params] ) - # GAZEBO_MODEL_PATH has to be correctly set for Gazebo to be able to find the model spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py', arguments=['-file', '/tmp/test_cart_position_pid.urdf', '-entity', 'cartpole'], diff --git a/gazebo_ros2_control_demos/launch/cart_example_velocity.launch.py b/gazebo_ros2_control_demos/launch/cart_example_velocity.launch.py index fa871851..cc5b0804 100644 --- a/gazebo_ros2_control_demos/launch/cart_example_velocity.launch.py +++ b/gazebo_ros2_control_demos/launch/cart_example_velocity.launch.py @@ -25,6 +25,8 @@ from launch_ros.actions import Node +import xacro + def generate_launch_description(): gazebo = IncludeLaunchDescription( @@ -38,12 +40,9 @@ def generate_launch_description(): xacro_file = os.path.join(gazebo_ros2_control_demos_path, 'urdf', 'test_cart_velocity.xacro.urdf') - robot_desc = Command('xacro %s' % xacro_file) - params = {'robot_description': robot_desc} - - context = LaunchContext() - command = Command('xacro %s -o /tmp/test_cart_velocity.urdf' % xacro_file) - command.perform(context) + doc = xacro.parse(open(xacro_file)) + xacro.process_doc(doc) + params = {'robot_description': doc.toxml()} node_robot_state_publisher = Node( package='robot_state_publisher', @@ -52,7 +51,6 @@ def generate_launch_description(): parameters=[params] ) - # GAZEBO_MODEL_PATH has to be correctly set for Gazebo to be able to find the model spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py', arguments=['-file', '/tmp/test_cart_velocity.urdf', '-entity', 'cartpole'], output='screen') From b01f47b7b15f6f1c8d4099d1ccb04d3abec137b1 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 28 Jul 2020 08:47:11 +0200 Subject: [PATCH 14/14] make linters happy Signed-off-by: ahcorde --- .../launch/cart_example_position.launch.py | 2 -- .../launch/cart_example_position_pid.launch.py | 2 -- .../launch/cart_example_velocity.launch.py | 2 -- 3 files changed, 6 deletions(-) diff --git a/gazebo_ros2_control_demos/launch/cart_example_position.launch.py b/gazebo_ros2_control_demos/launch/cart_example_position.launch.py index a8f99513..82ccd352 100644 --- a/gazebo_ros2_control_demos/launch/cart_example_position.launch.py +++ b/gazebo_ros2_control_demos/launch/cart_example_position.launch.py @@ -19,9 +19,7 @@ from launch import LaunchDescription from launch.actions import IncludeLaunchDescription -from launch.launch_context import LaunchContext from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import Command from launch_ros.actions import Node diff --git a/gazebo_ros2_control_demos/launch/cart_example_position_pid.launch.py b/gazebo_ros2_control_demos/launch/cart_example_position_pid.launch.py index b74e63a8..921dc632 100644 --- a/gazebo_ros2_control_demos/launch/cart_example_position_pid.launch.py +++ b/gazebo_ros2_control_demos/launch/cart_example_position_pid.launch.py @@ -19,9 +19,7 @@ from launch import LaunchDescription from launch.actions import IncludeLaunchDescription -from launch.launch_context import LaunchContext from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import Command from launch_ros.actions import Node diff --git a/gazebo_ros2_control_demos/launch/cart_example_velocity.launch.py b/gazebo_ros2_control_demos/launch/cart_example_velocity.launch.py index cc5b0804..95a90c6b 100644 --- a/gazebo_ros2_control_demos/launch/cart_example_velocity.launch.py +++ b/gazebo_ros2_control_demos/launch/cart_example_velocity.launch.py @@ -19,9 +19,7 @@ from launch import LaunchDescription from launch.actions import IncludeLaunchDescription -from launch.launch_context import LaunchContext from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import Command from launch_ros.actions import Node