From b11c3a5c8940e3e3de6dff30876b6e62ac1f402a Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Fri, 22 Jan 2021 09:01:49 +0000 Subject: [PATCH 01/57] First WIP of upload.py --- pmb2_description/CMakeLists.txt | 37 ++-- pmb2_description/package.xml | 18 +- pmb2_description/pmb2_description/__init__.py | 0 .../pmb2_description/launch_utils.py | 207 ++++++++++++++++++ .../pmb2_description/pmb2_launch_utils.py | 76 +++++++ pmb2_description/robots/upload.py | 65 ++++++ pmb2_robot/CMakeLists.txt | 6 +- pmb2_robot/package.xml | 12 +- 8 files changed, 391 insertions(+), 30 deletions(-) create mode 100644 pmb2_description/pmb2_description/__init__.py create mode 100644 pmb2_description/pmb2_description/launch_utils.py create mode 100644 pmb2_description/pmb2_description/pmb2_launch_utils.py create mode 100644 pmb2_description/robots/upload.py diff --git a/pmb2_description/CMakeLists.txt b/pmb2_description/CMakeLists.txt index 1b1d7a88..a64760c8 100644 --- a/pmb2_description/CMakeLists.txt +++ b/pmb2_description/CMakeLists.txt @@ -1,24 +1,33 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(pmb2_description) -find_package(catkin REQUIRED) +find_package(ament_cmake_auto REQUIRED) -catkin_package() +# PAL Robotics stricter build flags, other developers should not worry about this +find_package(ament_cmake_pal QUIET) + +ament_auto_find_build_dependencies() -foreach(dir config gazebo Media meshes robots urdf) - install(DIRECTORY ${dir}/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir}) -endforeach() ############# ## Testing ## ############# -if(CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) - foreach(laser_model false hokuyo sick-551 sick-561 sick-571) - foreach(rgbd_sensors false true) - add_rostest(test/test_pmb2.test ARGS laser_model:=${laser_model} rgbd_sensors:=${rgbd_sensors}) - endforeach(rgbd_sensors) - endforeach(laser_model) +if(BUILD_TESTING) + + find_package(ament_lint_auto REQUIRED) + + ament_lint_auto_find_test_dependencies() + + # find_package(rostest REQUIRED) + # foreach(laser_model false hokuyo sick-551 sick-561 sick-571) + # foreach(rgbd_sensors false true) + # add_rostest(test/test_pmb2.test ARGS laser_model:=${laser_model} rgbd_sensors:=${rgbd_sensors}) + # endforeach(rgbd_sensors) + # endforeach(laser_model) endif() + +# Install Python modules +ament_python_install_package(${PROJECT_NAME}) + +ament_auto_package(INSTALL_TO_SHARE config gazebo Media meshes robots urdf launch) diff --git a/pmb2_description/package.xml b/pmb2_description/package.xml index e0d3c133..ce2af738 100644 --- a/pmb2_description/package.xml +++ b/pmb2_description/package.xml @@ -1,24 +1,28 @@ - + pmb2_description - 3.0.14 + 4.0.0 Mechanical, kinematic, visual, etc. description of the PMB2 robot. The files in this package are parsed and used by a variety of other components. Most users will not interact directly with this package. - Procopio Stein - Jeremie Deray + Victor Lopez Apache License 2.0 Bence Magyar Enrique Fernandez - catkin + ament_cmake_auto + ament_cmake_python xacro - rostest - urdf_test + + + + ament_cmake + diff --git a/pmb2_description/pmb2_description/__init__.py b/pmb2_description/pmb2_description/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/pmb2_description/pmb2_description/launch_utils.py b/pmb2_description/pmb2_description/launch_utils.py new file mode 100644 index 00000000..f70510fc --- /dev/null +++ b/pmb2_description/pmb2_description/launch_utils.py @@ -0,0 +1,207 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import copy +import os + +from launch import Substitution, SomeSubstitutionsType, LaunchContext +from launch.substitutions import LaunchConfiguration +# import here to avoid loop +from launch.utilities import normalize_to_list_of_substitutions +import launch +import glob +import yaml +import tempfile + +from typing import Dict +from typing import List +from typing import Text +from typing import Optional + + +class DeclareLaunchArgumentChoice(DeclareLaunchArgument): + """ + Action that declares a launch argument with a limited range of choices + """ + def __init__( + self, + name: Text, + *, + default_value: Optional[SomeSubstitutionsType] = None, + description: Text = 'no description given', + arg_choices: List[Text], + **kwargs + ) -> None: + """Create a DeclareLaunchArgumentChoice action.""" + expanded_description = description + \ + " Valid choices are: " + str(arg_choices) + super().__init__(name, default_value=default_value, + description=expanded_description, **kwargs) + self.__arg_choices = arg_choices + + def execute(self, context: LaunchContext): + super().execute(context) + value = context.launch_configurations[self.name] + if value not in self.__arg_choices: + error_msg = "Argument '{}' provided value '{}' is not valid. Valid options are: {}".format( + self.name, value, self.__arg_choices) + # Argument not already set and no default value given, error. + self._DeclareLaunchArgument__logger.error(error_msg) + raise RuntimeError(error_msg) + + + +def _merge_dictionaries(dict1, dict2): + """ + Recursive merge dictionaries. + + :param dict1: Base dictionary to merge. + :param dict2: Dictionary to merge on top of base dictionary. + :return: Merged dictionary + """ + for key, val in dict1.items(): + if isinstance(val, dict): + dict2_node = dict2.setdefault(key, {}) + _merge_dictionaries(val, dict2_node) + else: + if key not in dict2: + dict2[key] = val + + return dict2 + + +def insert_ros_param_prefix(data, prefix): + if type(data) != dict: + return data + + for k in data.keys(): + if k == "ros__parameters": + d = {} + d[prefix] = copy.deepcopy(data[k]) + data[k] = d + else: + data[k] = insert_ros_param_prefix(data[k], prefix) + return data + +def merge_param_files(yaml_files): + """ + Substitution in ROS2 launch can only return a string. The way to combine multiple parameter files is to create a single temporary file and return the path to it, this path is passed as the "parameters" argument of a Node + + yaml_files is a list of either paths to yaml files (strings), or pairs of two strings (path, prefix), + so the file is loaded inside the provided prefix, inside the ros__parameters field + """ + concatenated_dict = {} + for e in yaml_files: + if type(e) == str: + yaml_file = e + prefix = None + else: + yaml_file = e[0] + prefix = e[1] + data = yaml.safe_load(open(yaml_file, 'r')) + if prefix: + data = insert_ros_param_prefix(data, prefix) + + _merge_dictionaries(concatenated_dict, data) + # Second arg takes precedence on merge, and result is stored there + concatenated_dict = data + rewritten_yaml = tempfile.NamedTemporaryFile(mode='w', delete=False) + yaml.dump(concatenated_dict, rewritten_yaml) + rewritten_yaml.close() + return rewritten_yaml.name + +def read_launch_argument(arg_name, context): + return launch.utilities.perform_substitutions(context, + [LaunchConfiguration(arg_name)]) + +class DeviceYamlParams(Substitution): + """ + Substitution that modifies the given YAML file. + Used in launch system + """ + + def __init__(self) -> None: + super().__init__() + """ + Construct the substitution + :param: source_file the original YAML file to modify + """ + + @property + def name(self) -> List[Substitution]: + """Getter for name.""" + return "potato name" + + def describe(self) -> Text: + """Return a description of this substitution as a string.""" + return 'potat describe' + + def get_motor_config_path(self): + return os.path.join(get_package_share_directory( + 'pal_deployer_cfg_tiago'), "config", "device", "motor", "") + + def get_sensor_config_path(self): + return os.path.join(get_package_share_directory( + 'pal_deployer_cfg_tiago'), "config", "device", "sensor", "") + + def get_wheel_motor_files(self, context: launch.LaunchContext): + # Read the provided variable for model name + wheel_model = read_launch_argument("wheel_model", context) + return [self.get_motor_config_path() + "wheel_left_motor_{}.yaml".format(wheel_model), + self.get_motor_config_path() + "wheel_right_motor_{}.yaml".format(wheel_model)] + + def get_device_files(self, context: launch.LaunchContext): + # Read the provided variable for model name + wheel_model = read_launch_argument("wheel_model", context) + sensors = [] + motors = ["wheel_left_motor_{}".format(wheel_model), + "wheel_right_motor_{}".format(wheel_model)] + arm = read_launch_argument("arm", context) + if arm == "True": + wrist_model = read_launch_argument("wrist_model", context) + if wrist_model == "wrist-2010": + wrist_suffix = "_wrist_2010" + else: + wrist_suffix = "_wrist_2017" + + motors += ["arm_1_motor", "arm_2_motor", "arm_3_motor", "arm_4_motor", + "arm_5_motor" + wrist_suffix, + "arm_6_motor" + wrist_suffix, + "arm_7_motor" + wrist_suffix] + + ft_sensor_model = read_launch_argument("ft_sensor_model", context) + if ft_sensor_model == "schunk-ft": + sensors = ["fts" + wrist_suffix] + full_path_list = [] + for motor in motors: + full_path_list.append([self.get_motor_config_path() + motor + ".yaml", "actuators"]) + + for sensor in sensors: + full_path_list.append([self.get_sensor_config_path() + sensor + ".yaml", "sensors"]) + + return full_path_list + + def perform(self, context: launch.LaunchContext) -> Text: + + yaml_files = [] + yaml_files += self.get_device_files(context) + return merge_param_files(yaml_files) + + +def generate_launch_description(): + # Arguments that are will be + configured_params = DeviceYamlParams() + + return LaunchDescription([ + *get_tiago_hw_arguments(wheel_model=True, + arm=True, wrist_model=True, end_effector_model=True, ft_sensor_model=True, + default_wheel_model="nadia"), + Node( + package='my_robot_tutorials', + executable='minimal_cpp_node', + parameters=[configured_params], + output='screen', + emulate_tty=True + ) + ]) diff --git a/pmb2_description/pmb2_description/pmb2_launch_utils.py b/pmb2_description/pmb2_description/pmb2_launch_utils.py new file mode 100644 index 00000000..bf3065ad --- /dev/null +++ b/pmb2_description/pmb2_description/pmb2_launch_utils.py @@ -0,0 +1,76 @@ +from pmb2_description.launch_utils import DeclareLaunchArgumentChoice + +def get_tiago_base_hw_arguments(wheel_model=False, + laser_model=False, + rgbd_sensors=False, + **kwargs): + """ TIAGo Base Hardware arguments helper function + Returns a list of the requested hardware LaunchArguments for tiago base + The default value can be configured passing an argument called default_NAME_OF_ARGUMENT + + example: + LaunchDescription([*get_tiago_base_hw_arguments(wheel_model=True, laser_model=True, default_laser_model='sick-571')]) + + """ + args = [] + if wheel_model: + args.append(DeclareLaunchArgumentChoice( + 'wheel_model', + default_value=kwargs.get("default_wheel_model"), + description='Wheel model, ', arg_choices=["nadia", "moog"])) + if laser_model: + args.append( + DeclareLaunchArgumentChoice( + 'laser_model', + default_value=kwargs.get("default_laser_model"), + description='Base laser model. ', arg_choices=["none", "sick-571", "ssick-561", "sick-551", "hokuyo"])) + if rgbd_sensors: + args.append( + DeclareLaunchArgumentChoice( + 'rgbd_sensors', + default_value=kwargs.get("default_rgbd_sensors"), + description='Whether the base has RGBD sensors or not. ', arg_choices=["True", "False"])) + return args + + +def get_tiago_hw_arguments(arm=False, + wrist_model=False, + end_effector_model=False, + ft_sensor_model=False, + **kwargs): + """ TIAGo Base Hardware arguments helper function + Returns a list of the requested hardware LaunchArguments for tiago base + The default value can be configured passing an argument called default_NAME_OF_ARGUMENT + + example: + LaunchDescription([*get_tiago_hw_arguments(wheel_model=True, laser_model=True, default_laser_model='sick-571')]) + + """ + args = get_tiago_base_hw_arguments( + rgbd_sensors=False, + **kwargs) # RGBD on top of base are impossible if torso is installed + if arm: + args.append(DeclareLaunchArgumentChoice( + 'arm', + default_value=kwargs.get("default_arm"), + description='Whether TIAGo has arm or not. ', arg_choices=["True", "False"])) + if wrist_model: + args.append( + DeclareLaunchArgumentChoice( + 'wrist_model', + default_value=kwargs.get("default_wrist_model"), + description='Wrist model. ', arg_choices=["wrist-2010", "wrist-2017"])) + if end_effector_model: + args.append( + DeclareLaunchArgumentChoice( + 'end_effector_model', + default_value=kwargs.get("default_end_effector_model"), + description='Wrist model. ', arg_choices=["pal-gripper", "pal-hey5", "schunk-wsg", "custom", "False"])) + if ft_sensor_model: + args.append( + DeclareLaunchArgumentChoice( + 'ft_sensor_model', + default_value=kwargs.get("default_ft_sensor_model"), + description='Wrist model. ', arg_choices=["schunk-ft", "False"])) + return args + diff --git a/pmb2_description/robots/upload.py b/pmb2_description/robots/upload.py new file mode 100644 index 00000000..0adc9430 --- /dev/null +++ b/pmb2_description/robots/upload.py @@ -0,0 +1,65 @@ +from launch import LaunchDescription, Substitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare, ExecutableInPackage +from launch.substitutions import Command, PathJoinSubstitution +from typing import List +from launch import Substitution, SomeSubstitutionsType, LaunchContext +from typing import Text + +from pmb2_description.launch_utils import read_launch_argument +from pmb2_description.pmb2_launch_utils import get_tiago_base_hw_arguments + + +class Pmb2XacroConfigSubstitution(Substitution): + """ + Substitution extracts the pmb2 hardware args and passes them as xacro variables + Used in launch system + """ + + def __init__(self) -> None: + super().__init__() + """ + Construct the substitution + :param: source_file the original YAML file t + """ + + @property + def name(self) -> List[Substitution]: + """Getter for name.""" + return "PMB2 Xacro Config" + + def describe(self) -> Text: + """Return a description of this substitution as a string.""" + return 'Parses pmb2 hardware launch arguments into xacro arguments potat describe' + + def perform(self, context: LaunchContext) -> Text: + """ + Generate the robot description and return it as a string + """ + + laser_model = read_launch_argument("laser_model", context) + rgbd_sensors = read_launch_argument("rgbd_sensors", context) + return " laser_model:=" + laser_model + " rgbd_sensors:=" + rgbd_sensors + +def generate_launch_description(): + + parameters={'robot_description': Command( + [ + ExecutableInPackage(package='xacro', executable="xacro"), + ' ', + PathJoinSubstitution([FindPackageShare('pmb2_description'), 'robots', 'pmb2.urdf.xacro']), + Pmb2XacroConfigSubstitution() + ]) + } + + rsp = Node(package='robot_state_publisher', + executable='robot_state_publisher', + output='both', + parameters=[parameters]) + return LaunchDescription([ + *get_tiago_base_hw_arguments(laser_model=True, + rgbd_sensors=True, + default_laser_model="sick-571", + default_rgbd_sensors="False"), + rsp + ]) diff --git a/pmb2_robot/CMakeLists.txt b/pmb2_robot/CMakeLists.txt index 3d096cbe..08a8f867 100644 --- a/pmb2_robot/CMakeLists.txt +++ b/pmb2_robot/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(pmb2_robot) -find_package(catkin REQUIRED) -catkin_metapackage() +find_package(ament_cmake REQUIRED) +ament_package(pmb2_robot) diff --git a/pmb2_robot/package.xml b/pmb2_robot/package.xml index c6f9b30b..02949c37 100644 --- a/pmb2_robot/package.xml +++ b/pmb2_robot/package.xml @@ -1,20 +1,20 @@ - + pmb2_robot - 3.0.14 + 4.0.0 PMB2 robot description and launch files - Procopio Stein - Jeremie Deray + Victor Lopez Apache License 2.0 Enrique Fernandez - catkin + ament_cmake pmb2_description pmb2_bringup pmb2_controller_configuration + - + ament_cmake From 92f40085c77a7178efaba9e85a4ab6103ec2616d Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Fri, 22 Jan 2021 09:02:04 +0000 Subject: [PATCH 02/57] Remove comments to workaround https://github.com/ros2/launch_ros/issues/214 --- pmb2_description/gazebo/gazebo.urdf.xacro | 18 ------------------ 1 file changed, 18 deletions(-) diff --git a/pmb2_description/gazebo/gazebo.urdf.xacro b/pmb2_description/gazebo/gazebo.urdf.xacro index 97e8b3c0..f3b8720c 100644 --- a/pmb2_description/gazebo/gazebo.urdf.xacro +++ b/pmb2_description/gazebo/gazebo.urdf.xacro @@ -42,30 +42,12 @@ - 1 1000.0 gaussian - 0.0 2e-4 From e1766e1d4340cb384f85d751bd1e9dba2df63669 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Mon, 25 Jan 2021 14:54:28 +0100 Subject: [PATCH 03/57] First working version --- pmb2_bringup/COLCON_IGNORE | 0 pmb2_controller_configuration/COLCON_IGNORE | 0 pmb2_description/gazebo/gazebo.urdf.xacro | 31 +++++----- .../robot_state_publisher.launch.py} | 0 pmb2_description/package.xml | 2 + .../pmb2_description/pmb2_launch_utils.py | 59 +++++++++++-------- .../urdf/sensors/range.gazebo.xacro | 23 ++++---- .../sensors/sick_tim571_laser.gazebo.xacro | 18 ++++-- pmb2_description/urdf/wheels/wheel.urdf.xacro | 5 +- pmb2_robot/CMakeLists.txt | 2 +- 10 files changed, 80 insertions(+), 60 deletions(-) create mode 100644 pmb2_bringup/COLCON_IGNORE create mode 100644 pmb2_controller_configuration/COLCON_IGNORE rename pmb2_description/{robots/upload.py => launch/robot_state_publisher.launch.py} (100%) diff --git a/pmb2_bringup/COLCON_IGNORE b/pmb2_bringup/COLCON_IGNORE new file mode 100644 index 00000000..e69de29b diff --git a/pmb2_controller_configuration/COLCON_IGNORE b/pmb2_controller_configuration/COLCON_IGNORE new file mode 100644 index 00000000..e69de29b diff --git a/pmb2_description/gazebo/gazebo.urdf.xacro b/pmb2_description/gazebo/gazebo.urdf.xacro index f3b8720c..de4323b4 100644 --- a/pmb2_description/gazebo/gazebo.urdf.xacro +++ b/pmb2_description/gazebo/gazebo.urdf.xacro @@ -16,25 +16,24 @@ name="pmb2"> - - - pal_hardware_gazebo/PalHardwareGazebo - - /${nsp} - - - - - 0.001 - + + gazebo_ros2_control/GazeboSystem + 0.001 + + + + + + + - base_footprint - ground_truth_odom - true - 100.0 + base_footprint + ground_truth_odom + true + 100.0 - /${nsp} + diff --git a/pmb2_description/robots/upload.py b/pmb2_description/launch/robot_state_publisher.launch.py similarity index 100% rename from pmb2_description/robots/upload.py rename to pmb2_description/launch/robot_state_publisher.launch.py diff --git a/pmb2_description/package.xml b/pmb2_description/package.xml index ce2af738..733a2a60 100644 --- a/pmb2_description/package.xml +++ b/pmb2_description/package.xml @@ -17,6 +17,8 @@ ament_cmake_python xacro + launch + launch_ros diff --git a/pmb2_description/pmb2_description/pmb2_launch_utils.py b/pmb2_description/pmb2_description/pmb2_launch_utils.py index bf3065ad..04642ec0 100644 --- a/pmb2_description/pmb2_description/pmb2_launch_utils.py +++ b/pmb2_description/pmb2_description/pmb2_launch_utils.py @@ -1,9 +1,12 @@ -from pmb2_description.launch_utils import DeclareLaunchArgumentChoice +from launch.actions import DeclareLaunchArgument def get_tiago_base_hw_arguments(wheel_model=False, laser_model=False, rgbd_sensors=False, - **kwargs): + default_wheel_model='moog', + default_laser_model='sick-571', + default_rgbd_sensors=False #Maybe this needs to be a string + ): """ TIAGo Base Hardware arguments helper function Returns a list of the requested hardware LaunchArguments for tiago base The default value can be configured passing an argument called default_NAME_OF_ARGUMENT @@ -14,22 +17,22 @@ def get_tiago_base_hw_arguments(wheel_model=False, """ args = [] if wheel_model: - args.append(DeclareLaunchArgumentChoice( + args.append(DeclareLaunchArgument( 'wheel_model', - default_value=kwargs.get("default_wheel_model"), - description='Wheel model, ', arg_choices=["nadia", "moog"])) + default_value=default_wheel_model, + description='Wheel model, ', choices=["nadia", "moog"])) if laser_model: args.append( - DeclareLaunchArgumentChoice( + DeclareLaunchArgument( 'laser_model', - default_value=kwargs.get("default_laser_model"), - description='Base laser model. ', arg_choices=["none", "sick-571", "ssick-561", "sick-551", "hokuyo"])) + default_value=default_laser_model, + description='Base laser model. ', choices=["sick-571", "sick-561", "sick-551", "hokuyo"])) if rgbd_sensors: args.append( - DeclareLaunchArgumentChoice( + DeclareLaunchArgument( 'rgbd_sensors', - default_value=kwargs.get("default_rgbd_sensors"), - description='Whether the base has RGBD sensors or not. ', arg_choices=["True", "False"])) + default_value=default_rgbd_sensors, + description='Whether the base has RGBD sensors or not. ', choices=["True", "False"])) return args @@ -37,11 +40,17 @@ def get_tiago_hw_arguments(arm=False, wrist_model=False, end_effector_model=False, ft_sensor_model=False, + default_arm=True, + default_wrist_model="wrist-2017", + default_end_effector_model="pal-hey5", + default_ft_sensor_model="schunk-ft", **kwargs): - """ TIAGo Base Hardware arguments helper function - Returns a list of the requested hardware LaunchArguments for tiago base + """ TIAGo Hardware arguments helper function + Returns a list of the requested hardware LaunchArguments for tiago The default value can be configured passing an argument called default_NAME_OF_ARGUMENT + Check get_tiago_hw_arguments for more options + example: LaunchDescription([*get_tiago_hw_arguments(wheel_model=True, laser_model=True, default_laser_model='sick-571')]) @@ -50,27 +59,27 @@ def get_tiago_hw_arguments(arm=False, rgbd_sensors=False, **kwargs) # RGBD on top of base are impossible if torso is installed if arm: - args.append(DeclareLaunchArgumentChoice( + args.append(DeclareLaunchArgument( 'arm', - default_value=kwargs.get("default_arm"), - description='Whether TIAGo has arm or not. ', arg_choices=["True", "False"])) + default_value=default_arm, + description='Whether TIAGo has arm or not. ', choices=["True", "False"])) if wrist_model: args.append( - DeclareLaunchArgumentChoice( + DeclareLaunchArgument( 'wrist_model', - default_value=kwargs.get("default_wrist_model"), - description='Wrist model. ', arg_choices=["wrist-2010", "wrist-2017"])) + default_value=default_wrist_model, + description='Wrist model. ', choices=["wrist-2010", "wrist-2017"])) if end_effector_model: args.append( - DeclareLaunchArgumentChoice( + DeclareLaunchArgument( 'end_effector_model', - default_value=kwargs.get("default_end_effector_model"), - description='Wrist model. ', arg_choices=["pal-gripper", "pal-hey5", "schunk-wsg", "custom", "False"])) + default_value=default_end_effector_model, + description='Wrist model. ', choices=["pal-gripper", "pal-hey5", "schunk-wsg", "custom", "False"])) if ft_sensor_model: args.append( - DeclareLaunchArgumentChoice( + DeclareLaunchArgument( 'ft_sensor_model', - default_value=kwargs.get("default_ft_sensor_model"), - description='Wrist model. ', arg_choices=["schunk-ft", "False"])) + default_value=default_ft_sensor_model, + description='Wrist model. ', choices=["schunk-ft", "False"])) return args diff --git a/pmb2_description/urdf/sensors/range.gazebo.xacro b/pmb2_description/urdf/sensors/range.gazebo.xacro index f723877c..e897768a 100644 --- a/pmb2_description/urdf/sensors/range.gazebo.xacro +++ b/pmb2_description/urdf/sensors/range.gazebo.xacro @@ -37,17 +37,20 @@ ${maxRange} 0.01 + + gaussian + 0.0 + 0.005 + - - 0.005 - true - ${update_rate} - ${ros_topic} - ${name}_link - ${minRange} - ${maxRange} - ${fov} - ${radiation} + + + distance + ~/out:=${ros_topic} + + sensor_msgs/Range + ${radiation} + ${name}_link Gazebo/DarkGrey diff --git a/pmb2_description/urdf/sensors/sick_tim571_laser.gazebo.xacro b/pmb2_description/urdf/sensors/sick_tim571_laser.gazebo.xacro index 539961ad..6e89a2f6 100644 --- a/pmb2_description/urdf/sensors/sick_tim571_laser.gazebo.xacro +++ b/pmb2_description/urdf/sensors/sick_tim571_laser.gazebo.xacro @@ -34,13 +34,19 @@ 25.0 0.001 + + gaussian + 0.0 + 0.06 + - - 0.06 - true - ${update_rate} - ${ros_topic} - ${name}_link + + + lidar + ~/out:=${ros_topic} + + sensor_msgs/LaserScan + ${name}_link Gazebo/DarkGrey diff --git a/pmb2_description/urdf/wheels/wheel.urdf.xacro b/pmb2_description/urdf/wheels/wheel.urdf.xacro index 25811dcf..3d851cb7 100644 --- a/pmb2_description/urdf/wheels/wheel.urdf.xacro +++ b/pmb2_description/urdf/wheels/wheel.urdf.xacro @@ -13,6 +13,7 @@ Creative Commons, 444 Castro Street, Suite 900, Mountain View, California, 94041 + @@ -75,8 +76,8 @@ Creative Commons, 444 Castro Street, Suite 900, Mountain View, California, 94041 - - + + diff --git a/pmb2_robot/CMakeLists.txt b/pmb2_robot/CMakeLists.txt index 08a8f867..d5a625a5 100644 --- a/pmb2_robot/CMakeLists.txt +++ b/pmb2_robot/CMakeLists.txt @@ -1,4 +1,4 @@ cmake_minimum_required(VERSION 3.5) project(pmb2_robot) find_package(ament_cmake REQUIRED) -ament_package(pmb2_robot) +ament_package() From c46179832d7a3b6dff5bd20925696521bc5897dc Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Tue, 26 Jan 2021 13:50:55 +0000 Subject: [PATCH 04/57] Add pmb2_controller_configuration --- pmb2_controller_configuration/CMakeLists.txt | 14 ++-- pmb2_controller_configuration/COLCON_IGNORE | 0 .../config/default_controllers.yaml | 74 +++++++++++++++++++ pmb2_controller_configuration/package.xml | 9 ++- 4 files changed, 87 insertions(+), 10 deletions(-) delete mode 100644 pmb2_controller_configuration/COLCON_IGNORE create mode 100644 pmb2_controller_configuration/config/default_controllers.yaml diff --git a/pmb2_controller_configuration/CMakeLists.txt b/pmb2_controller_configuration/CMakeLists.txt index 3cdf7ad4..b551460d 100644 --- a/pmb2_controller_configuration/CMakeLists.txt +++ b/pmb2_controller_configuration/CMakeLists.txt @@ -1,11 +1,11 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(pmb2_controller_configuration) -find_package(catkin REQUIRED) +find_package(ament_cmake_auto REQUIRED) -catkin_package() +# PAL Robotics stricter build flags, other developers should not worry about this +find_package(ament_cmake_pal QUIET) -foreach(dir config launch) - install(DIRECTORY ${dir}/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir}) -endforeach() +ament_auto_find_build_dependencies() + +ament_auto_package(INSTALL_TO_SHARE config launch) diff --git a/pmb2_controller_configuration/COLCON_IGNORE b/pmb2_controller_configuration/COLCON_IGNORE deleted file mode 100644 index e69de29b..00000000 diff --git a/pmb2_controller_configuration/config/default_controllers.yaml b/pmb2_controller_configuration/config/default_controllers.yaml new file mode 100644 index 00000000..1b6ce072 --- /dev/null +++ b/pmb2_controller_configuration/config/default_controllers.yaml @@ -0,0 +1,74 @@ +gazebo_controller_manager: + ros__parameters: + mobile_base_controller: + type: diff_drive_controller/DiffDriveController + forward_command_controller_position: + type: forward_command_controller/ForwardCommandController + joint_state_controller: + type: joint_state_controller/JointStateController + +joint_state_controller: + ros__parameters: + type: joint_state_controller/JointStateController + publish_rate: 50.0 + +mobile_base_controller: + ros__parameters: + type: diff_drive_controller/DiffDriveController + left_wheel_names : ['wheel_left_joint'] + right_wheel_names : ['wheel_right_joint'] + #wheels_per_side: 1 + + # In ROS1 these physical properties were parsed from the URDF + wheel_radius: 0.0985 + wheel_separation: 0.4044 + + wheel_separation_multiplier: 1.0 + left_wheel_radius_multiplier: 1.0 + right_wheel_radius_multiplier: 1.0 + + # The params below were used in ROS1 but not yet in ROS2 + + publish_rate: 50.0 + pose_covariance_diagonal : [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] + twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] + + enable_pose_covariance_update: true + error_constant_left : 0.001 + error_constant_right: 0.001 + + # Velocity commands timeout [s] + cmd_vel_timeout: 0.25 + + # Base frame_id + base_frame_id: base_footprint + + # Preserve turning radius when limiting speed/acceleration/jerk + preserve_turning_radius: true + + # Publish limited velocity + publish_cmd: true + + # Publish wheel data + publish_wheel_data: true + + # Velocity and acceleration limits + # Whenever a min_* is unspecified, default to -max_* + linear: + x: + has_velocity_limits : true + max_velocity : 1.0 # m/s + min_velocity : -0.2 # m/s + has_acceleration_limits: true + max_acceleration : 1.0 # m/s^2 + has_jerk_limits : false + max_jerk : 5.0 # m/s^3 + angular: + z: + has_velocity_limits : true + max_velocity : 2.09 # 120.0º + has_acceleration_limits: true + max_acceleration : 2.09 # 120.0º + has_jerk_limits : false + max_jerk : 10.47 # 600.0º + diff --git a/pmb2_controller_configuration/package.xml b/pmb2_controller_configuration/package.xml index 3df90fb0..076c776d 100644 --- a/pmb2_controller_configuration/package.xml +++ b/pmb2_controller_configuration/package.xml @@ -4,16 +4,19 @@ 3.0.14 Launch files and scripts needed to configure the controllers of the PMB2 robot. - Procopio Stein - Jeremie Deray + Victor Lopez + Federico Nardi Apache License 2.0 Enrique Fernandez - catkin + ament_cmake controller_manager diff_drive_controller joint_state_controller imu_sensor_controller + + ament_cmake + From a20746614675b51171bd3c52ba2c6db24c093128 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Tue, 2 Feb 2021 09:33:54 +0000 Subject: [PATCH 05/57] Add wheel ros2_control file --- .../urdf/wheels/wheel.ros2_control.xacro | 33 +++++++++++++++++++ 1 file changed, 33 insertions(+) create mode 100644 pmb2_description/urdf/wheels/wheel.ros2_control.xacro diff --git a/pmb2_description/urdf/wheels/wheel.ros2_control.xacro b/pmb2_description/urdf/wheels/wheel.ros2_control.xacro new file mode 100644 index 00000000..f0c7e6b8 --- /dev/null +++ b/pmb2_description/urdf/wheels/wheel.ros2_control.xacro @@ -0,0 +1,33 @@ + + + + + + + + + -1 + 1 + + + + + transmission_interface/SimpleTransmission + + 1.0 + + + hardware_interface/VelocityJointInterface + hardware_interface/JointStateInterface + + + + + From 751bea058145f3d4d8ad73f7d12bbf3a07d0eecf Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Tue, 2 Feb 2021 12:21:35 +0000 Subject: [PATCH 06/57] Update how ros2 gazebo plugin is loaded --- pmb2_description/gazebo/gazebo.urdf.xacro | 1 - pmb2_description/urdf/wheels/wheel.ros2_control.xacro | 3 +++ pmb2_description/urdf/wheels/wheel.urdf.xacro | 2 +- 3 files changed, 4 insertions(+), 2 deletions(-) diff --git a/pmb2_description/gazebo/gazebo.urdf.xacro b/pmb2_description/gazebo/gazebo.urdf.xacro index de4323b4..bba665f8 100644 --- a/pmb2_description/gazebo/gazebo.urdf.xacro +++ b/pmb2_description/gazebo/gazebo.urdf.xacro @@ -17,7 +17,6 @@ - gazebo_ros2_control/GazeboSystem 0.001 diff --git a/pmb2_description/urdf/wheels/wheel.ros2_control.xacro b/pmb2_description/urdf/wheels/wheel.ros2_control.xacro index f0c7e6b8..06dc1921 100644 --- a/pmb2_description/urdf/wheels/wheel.ros2_control.xacro +++ b/pmb2_description/urdf/wheels/wheel.ros2_control.xacro @@ -11,6 +11,9 @@ + + gazebo_ros2_control/GazeboSystem + -1 diff --git a/pmb2_description/urdf/wheels/wheel.urdf.xacro b/pmb2_description/urdf/wheels/wheel.urdf.xacro index 3d851cb7..66f58e1c 100644 --- a/pmb2_description/urdf/wheels/wheel.urdf.xacro +++ b/pmb2_description/urdf/wheels/wheel.urdf.xacro @@ -77,7 +77,7 @@ Creative Commons, 444 Castro Street, Suite 900, Mountain View, California, 94041 - + From ea72b0a4e044c6e6ed06664769bf09032a2b4eac Mon Sep 17 00:00:00 2001 From: Victor Lopez <3469405+v-lopez@users.noreply.github.com> Date: Wed, 3 Feb 2021 09:50:37 +0100 Subject: [PATCH 07/57] Update default_controllers.yaml Update gazebo controller name --- pmb2_controller_configuration/config/default_controllers.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pmb2_controller_configuration/config/default_controllers.yaml b/pmb2_controller_configuration/config/default_controllers.yaml index 1b6ce072..f29b7a86 100644 --- a/pmb2_controller_configuration/config/default_controllers.yaml +++ b/pmb2_controller_configuration/config/default_controllers.yaml @@ -1,4 +1,4 @@ -gazebo_controller_manager: +controller_manager: ros__parameters: mobile_base_controller: type: diff_drive_controller/DiffDriveController From b7fd99517b399c136ca2af4cc0d7f3446511da29 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Thu, 4 Feb 2021 08:25:28 +0100 Subject: [PATCH 08/57] Add default_controllers.launch.py --- .../launch/default_controllers.launch.py | 67 +++++++++++++++++++ 1 file changed, 67 insertions(+) create mode 100644 pmb2_controller_configuration/launch/default_controllers.launch.py diff --git a/pmb2_controller_configuration/launch/default_controllers.launch.py b/pmb2_controller_configuration/launch/default_controllers.launch.py new file mode 100644 index 00000000..ddc92736 --- /dev/null +++ b/pmb2_controller_configuration/launch/default_controllers.launch.py @@ -0,0 +1,67 @@ +# Copyright (c) 2020 PAL Robotics S.L. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# 3. Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from this +# software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +# IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +# TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +# PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +# HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, +# DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY +# THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +import os +from os import environ, pathsep + +from ament_index_python.packages import get_package_share_directory, get_package_prefix + +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable, DeclareLaunchArgument +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + +from launch_ros.actions import Node + +import xacro + + +def generate_load_controller_launch_description(controller_name, pkg_name, controller_params_file): + + pkg_path = get_package_share_directory('pmb2_description') + param_file_path = os.path.join(pkg_path, "config", "default_controllers.yaml") + + spawner = Node(package='controller_manager', executable='spawner.py', + arguments=[controller_name, + '--controller-manager', LaunchConfiguration('controller_manager_name'), + '--param-file', param_file_path], + output='screen') + + return LaunchDescription([ + DeclareLaunchArgument( + 'controller_manager_name', default_value='controller_manager', + description='Controller manager node name' + ), + spawner, + ]) + +def generate_launch_description(): + + return generate_load_controller_launch_description(controller_name='mobile_base_controller', + controller_type='', + pkg_name='pmb2_controller_configuration', + controller_params_file=os.path.join('config', 'default_controllers.yaml')) + From e547a8ac84044ea924a3bb593e96ee4d8157247a Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Thu, 4 Feb 2021 08:46:55 +0100 Subject: [PATCH 09/57] More fixes to default_controllers --- .../launch/default_controllers.launch.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/pmb2_controller_configuration/launch/default_controllers.launch.py b/pmb2_controller_configuration/launch/default_controllers.launch.py index ddc92736..ed9b05c0 100644 --- a/pmb2_controller_configuration/launch/default_controllers.launch.py +++ b/pmb2_controller_configuration/launch/default_controllers.launch.py @@ -39,13 +39,14 @@ import xacro -def generate_load_controller_launch_description(controller_name, pkg_name, controller_params_file): +def generate_load_controller_launch_description(controller_name, controller_type, pkg_name, controller_params_file): - pkg_path = get_package_share_directory('pmb2_description') + pkg_path = get_package_share_directory(pkg_name) param_file_path = os.path.join(pkg_path, "config", "default_controllers.yaml") spawner = Node(package='controller_manager', executable='spawner.py', arguments=[controller_name, + '--controller-type', controller_type, '--controller-manager', LaunchConfiguration('controller_manager_name'), '--param-file', param_file_path], output='screen') @@ -61,7 +62,7 @@ def generate_load_controller_launch_description(controller_name, pkg_name, contr def generate_launch_description(): return generate_load_controller_launch_description(controller_name='mobile_base_controller', - controller_type='', + controller_type='diff_drive_controller/DiffDriveController', pkg_name='pmb2_controller_configuration', controller_params_file=os.path.join('config', 'default_controllers.yaml')) From ea49913a75b12aacc159d1a815aa909f2d4f861c Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Thu, 4 Feb 2021 09:00:46 +0100 Subject: [PATCH 10/57] Fixes to gazebo ros2 control param changes --- .../config/gazebo_controller_manager_cfg.yaml | 3 +++ pmb2_description/gazebo/gazebo.urdf.xacro | 3 ++- 2 files changed, 5 insertions(+), 1 deletion(-) create mode 100644 pmb2_controller_configuration/config/gazebo_controller_manager_cfg.yaml diff --git a/pmb2_controller_configuration/config/gazebo_controller_manager_cfg.yaml b/pmb2_controller_configuration/config/gazebo_controller_manager_cfg.yaml new file mode 100644 index 00000000..f560a885 --- /dev/null +++ b/pmb2_controller_configuration/config/gazebo_controller_manager_cfg.yaml @@ -0,0 +1,3 @@ +controller_manager: + ros__parameters: + update_rate: 100 # Hz diff --git a/pmb2_description/gazebo/gazebo.urdf.xacro b/pmb2_description/gazebo/gazebo.urdf.xacro index bba665f8..1a4d8d6b 100644 --- a/pmb2_description/gazebo/gazebo.urdf.xacro +++ b/pmb2_description/gazebo/gazebo.urdf.xacro @@ -17,7 +17,8 @@ - 0.001 + + $(find pmb2_controller_configuration)/config/gazebo_controller_manager_cfg.yaml From 8665abfd273f455e40ec8d02165d38d4f3869d3d Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Tue, 9 Feb 2021 11:40:43 +0100 Subject: [PATCH 11/57] Split default_controllers launch file --- .../config/default_controllers.yaml | 74 ------------- .../config/joint_state_controller.yaml | 6 + .../config/mobile_base_controller.yaml | 104 ++++++++++-------- .../launch/default_controllers.launch.py | 45 ++------ .../launch/joint_state_controller.launch.py | 69 ++++++++++++ .../launch/mobile_base_controller.launch.py | 69 ++++++++++++ 6 files changed, 215 insertions(+), 152 deletions(-) delete mode 100644 pmb2_controller_configuration/config/default_controllers.yaml create mode 100644 pmb2_controller_configuration/config/joint_state_controller.yaml create mode 100644 pmb2_controller_configuration/launch/joint_state_controller.launch.py create mode 100644 pmb2_controller_configuration/launch/mobile_base_controller.launch.py diff --git a/pmb2_controller_configuration/config/default_controllers.yaml b/pmb2_controller_configuration/config/default_controllers.yaml deleted file mode 100644 index f29b7a86..00000000 --- a/pmb2_controller_configuration/config/default_controllers.yaml +++ /dev/null @@ -1,74 +0,0 @@ -controller_manager: - ros__parameters: - mobile_base_controller: - type: diff_drive_controller/DiffDriveController - forward_command_controller_position: - type: forward_command_controller/ForwardCommandController - joint_state_controller: - type: joint_state_controller/JointStateController - -joint_state_controller: - ros__parameters: - type: joint_state_controller/JointStateController - publish_rate: 50.0 - -mobile_base_controller: - ros__parameters: - type: diff_drive_controller/DiffDriveController - left_wheel_names : ['wheel_left_joint'] - right_wheel_names : ['wheel_right_joint'] - #wheels_per_side: 1 - - # In ROS1 these physical properties were parsed from the URDF - wheel_radius: 0.0985 - wheel_separation: 0.4044 - - wheel_separation_multiplier: 1.0 - left_wheel_radius_multiplier: 1.0 - right_wheel_radius_multiplier: 1.0 - - # The params below were used in ROS1 but not yet in ROS2 - - publish_rate: 50.0 - pose_covariance_diagonal : [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] - twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] - - enable_pose_covariance_update: true - error_constant_left : 0.001 - error_constant_right: 0.001 - - # Velocity commands timeout [s] - cmd_vel_timeout: 0.25 - - # Base frame_id - base_frame_id: base_footprint - - # Preserve turning radius when limiting speed/acceleration/jerk - preserve_turning_radius: true - - # Publish limited velocity - publish_cmd: true - - # Publish wheel data - publish_wheel_data: true - - # Velocity and acceleration limits - # Whenever a min_* is unspecified, default to -max_* - linear: - x: - has_velocity_limits : true - max_velocity : 1.0 # m/s - min_velocity : -0.2 # m/s - has_acceleration_limits: true - max_acceleration : 1.0 # m/s^2 - has_jerk_limits : false - max_jerk : 5.0 # m/s^3 - angular: - z: - has_velocity_limits : true - max_velocity : 2.09 # 120.0º - has_acceleration_limits: true - max_acceleration : 2.09 # 120.0º - has_jerk_limits : false - max_jerk : 10.47 # 600.0º - diff --git a/pmb2_controller_configuration/config/joint_state_controller.yaml b/pmb2_controller_configuration/config/joint_state_controller.yaml new file mode 100644 index 00000000..f6666b7d --- /dev/null +++ b/pmb2_controller_configuration/config/joint_state_controller.yaml @@ -0,0 +1,6 @@ +joint_state_controller: + ros__parameters: + type: joint_state_controller/JointStateController + publish_rate: 50.0 + + diff --git a/pmb2_controller_configuration/config/mobile_base_controller.yaml b/pmb2_controller_configuration/config/mobile_base_controller.yaml index c5c24ca3..c5fd6ca7 100644 --- a/pmb2_controller_configuration/config/mobile_base_controller.yaml +++ b/pmb2_controller_configuration/config/mobile_base_controller.yaml @@ -1,46 +1,60 @@ mobile_base_controller: - type : "diff_drive_controller/DiffDriveController" - left_wheel : 'wheel_left_joint' - right_wheel : 'wheel_right_joint' - publish_rate: 50.0 - pose_covariance_diagonal : [0.001, 0.001, 1.0e-3, 1.0e-3, 1.0e-3, 0.01] - twist_covariance_diagonal: [0.001, 0.001, 1.0e-3, 1.0e-3, 1.0e-3, 0.01] - - enable_pose_covariance_update: true - error_constant_left : 0.001 - error_constant_right: 0.001 - - # Velocity commands timeout [s] - cmd_vel_timeout: 0.25 - - # Base frame_id - base_frame_id: base_footprint - - # Preserve turning radius when limiting speed/acceleration/jerk - preserve_turning_radius: true - - # Publish limited velocity - publish_cmd: true - - # Publish wheel data - publish_wheel_data: true - - # Velocity and acceleration limits - # Whenever a min_* is unspecified, default to -max_* - linear: - x: - has_velocity_limits : true - max_velocity : 1.0 # m/s - min_velocity : -0.2 # m/s - has_acceleration_limits: true - max_acceleration : 1.0 # m/s^2 - has_jerk_limits : false - max_jerk : 5.0 # m/s^3 - angular: - z: - has_velocity_limits : true - max_velocity : !degrees 120.0 - has_acceleration_limits: true - max_acceleration : !degrees 120.0 - has_jerk_limits : false - max_jerk : !degrees 600.0 + ros__parameters: + type: diff_drive_controller/DiffDriveController + left_wheel_names : ['wheel_left_joint'] + right_wheel_names : ['wheel_right_joint'] + #wheels_per_side: 1 + + # In ROS1 these physical properties were parsed from the URDF + wheel_radius: 0.0985 + wheel_separation: 0.4044 + + wheel_separation_multiplier: 1.0 + left_wheel_radius_multiplier: 1.0 + right_wheel_radius_multiplier: 1.0 + + # The params below were used in ROS1 but not yet in ROS2 + + publish_rate: 50.0 + pose_covariance_diagonal : [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] + twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] + + enable_pose_covariance_update: true + error_constant_left : 0.001 + error_constant_right: 0.001 + + # Velocity commands timeout [s] + cmd_vel_timeout: 0.25 + + # Base frame_id + base_frame_id: base_footprint + + # Preserve turning radius when limiting speed/acceleration/jerk + preserve_turning_radius: true + + # Publish limited velocity + publish_cmd: true + + # Publish wheel data + publish_wheel_data: true + + # Velocity and acceleration limits + # Whenever a min_* is unspecified, default to -max_* + linear: + x: + has_velocity_limits : true + max_velocity : 1.0 # m/s + min_velocity : -0.2 # m/s + has_acceleration_limits: true + max_acceleration : 1.0 # m/s^2 + has_jerk_limits : false + max_jerk : 5.0 # m/s^3 + angular: + z: + has_velocity_limits : true + max_velocity : 2.09 # 120.0º + has_acceleration_limits: true + max_acceleration : 2.09 # 120.0º + has_jerk_limits : false + max_jerk : 10.47 # 600.0º + diff --git a/pmb2_controller_configuration/launch/default_controllers.launch.py b/pmb2_controller_configuration/launch/default_controllers.launch.py index ed9b05c0..23fc54a5 100644 --- a/pmb2_controller_configuration/launch/default_controllers.launch.py +++ b/pmb2_controller_configuration/launch/default_controllers.launch.py @@ -25,44 +25,23 @@ # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. import os -from os import environ, pathsep - -from ament_index_python.packages import get_package_share_directory, get_package_prefix +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable, DeclareLaunchArgument +from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration - -from launch_ros.actions import Node - -import xacro - - -def generate_load_controller_launch_description(controller_name, controller_type, pkg_name, controller_params_file): - pkg_path = get_package_share_directory(pkg_name) - param_file_path = os.path.join(pkg_path, "config", "default_controllers.yaml") - - spawner = Node(package='controller_manager', executable='spawner.py', - arguments=[controller_name, - '--controller-type', controller_type, - '--controller-manager', LaunchConfiguration('controller_manager_name'), - '--param-file', param_file_path], - output='screen') +def generate_launch_description(): return LaunchDescription([ - DeclareLaunchArgument( - 'controller_manager_name', default_value='controller_manager', - description='Controller manager node name' + IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('pmb2_controller_configuration'), 'launch', + 'mobile_base_controller.launch.py')]), ), - spawner, - ]) - -def generate_launch_description(): - - return generate_load_controller_launch_description(controller_name='mobile_base_controller', - controller_type='diff_drive_controller/DiffDriveController', - pkg_name='pmb2_controller_configuration', - controller_params_file=os.path.join('config', 'default_controllers.yaml')) + IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('pmb2_controller_configuration'), 'launch', + 'joint_state_controller.launch.py')]), + )]) diff --git a/pmb2_controller_configuration/launch/joint_state_controller.launch.py b/pmb2_controller_configuration/launch/joint_state_controller.launch.py new file mode 100644 index 00000000..efb2d62a --- /dev/null +++ b/pmb2_controller_configuration/launch/joint_state_controller.launch.py @@ -0,0 +1,69 @@ +# Copyright (c) 2020 PAL Robotics S.L. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# 3. Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from this +# software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +# IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +# TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +# PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +# HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, +# DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY +# THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +import os +from os import environ, pathsep + +from ament_index_python.packages import get_package_share_directory, get_package_prefix + +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable, DeclareLaunchArgument +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + +from launch_ros.actions import Node + +import xacro + + +# Todo put this somewhere reusable, see https://github.com/ros-controls/ros2_control/issues/320 +def generate_load_controller_launch_description(controller_name, controller_type, pkg_name, controller_params_file): + + pkg_path = get_package_share_directory(pkg_name) + param_file_path = os.path.join(pkg_path, controller_params_file) + + spawner = Node(package='controller_manager', executable='spawner.py', + arguments=[controller_name, + '--controller-type', controller_type, + '--controller-manager', LaunchConfiguration('controller_manager_name'), + '--param-file', param_file_path], + output='screen') + + return LaunchDescription([ + DeclareLaunchArgument( + 'controller_manager_name', default_value='controller_manager', + description='Controller manager node name' + ), + spawner, + ]) + +def generate_launch_description(): + + return generate_load_controller_launch_description(controller_name='joint_state_controller', + controller_type='joint_state_controller/JointStateController', + pkg_name='pmb2_controller_configuration', + controller_params_file=os.path.join('config', 'joint_state_controller.yaml')) + diff --git a/pmb2_controller_configuration/launch/mobile_base_controller.launch.py b/pmb2_controller_configuration/launch/mobile_base_controller.launch.py new file mode 100644 index 00000000..222ebb64 --- /dev/null +++ b/pmb2_controller_configuration/launch/mobile_base_controller.launch.py @@ -0,0 +1,69 @@ +# Copyright (c) 2020 PAL Robotics S.L. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# 3. Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from this +# software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +# IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +# TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +# PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +# HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, +# DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY +# THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +import os +from os import environ, pathsep + +from ament_index_python.packages import get_package_share_directory, get_package_prefix + +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable, DeclareLaunchArgument +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + +from launch_ros.actions import Node + +import xacro + + +# Todo put this somewhere reusable, see https://github.com/ros-controls/ros2_control/issues/320 +def generate_load_controller_launch_description(controller_name, controller_type, pkg_name, controller_params_file): + + pkg_path = get_package_share_directory(pkg_name) + param_file_path = os.path.join(pkg_path, controller_params_file) + + spawner = Node(package='controller_manager', executable='spawner.py', + arguments=[controller_name, + '--controller-type', controller_type, + '--controller-manager', LaunchConfiguration('controller_manager_name'), + '--param-file', param_file_path], + output='screen') + + return LaunchDescription([ + DeclareLaunchArgument( + 'controller_manager_name', default_value='controller_manager', + description='Controller manager node name' + ), + spawner, + ]) + +def generate_launch_description(): + + return generate_load_controller_launch_description(controller_name='mobile_base_controller', + controller_type='diff_drive_controller/DiffDriveController', + pkg_name='pmb2_controller_configuration', + controller_params_file=os.path.join('config', 'mobile_base_controller.yaml')) + From b75db7b77c4219299904a7120ac52a0d7db59f31 Mon Sep 17 00:00:00 2001 From: Jordan Palacios Date: Tue, 9 Feb 2021 18:02:02 +0100 Subject: [PATCH 12/57] All joints now form part of a single ros2_control system --- pmb2_description/CMakeLists.txt | 2 +- pmb2_description/robots/pmb2.urdf.xacro | 4 ++ .../ros2_control/ros2_control.urdf.xacro | 25 +++++++++++ .../urdf/wheels/wheel.ros2_control.xacro | 41 ++++++++----------- pmb2_description/urdf/wheels/wheel.urdf.xacro | 8 +--- 5 files changed, 50 insertions(+), 30 deletions(-) create mode 100644 pmb2_description/ros2_control/ros2_control.urdf.xacro diff --git a/pmb2_description/CMakeLists.txt b/pmb2_description/CMakeLists.txt index a64760c8..6511c79e 100644 --- a/pmb2_description/CMakeLists.txt +++ b/pmb2_description/CMakeLists.txt @@ -30,4 +30,4 @@ endif() # Install Python modules ament_python_install_package(${PROJECT_NAME}) -ament_auto_package(INSTALL_TO_SHARE config gazebo Media meshes robots urdf launch) +ament_auto_package(INSTALL_TO_SHARE config gazebo launch Media meshes robots ros2_control urdf) diff --git a/pmb2_description/robots/pmb2.urdf.xacro b/pmb2_description/robots/pmb2.urdf.xacro index 3602b502..0a4bc112 100644 --- a/pmb2_description/robots/pmb2.urdf.xacro +++ b/pmb2_description/robots/pmb2.urdf.xacro @@ -28,6 +28,10 @@ + + + + diff --git a/pmb2_description/ros2_control/ros2_control.urdf.xacro b/pmb2_description/ros2_control/ros2_control.urdf.xacro new file mode 100644 index 00000000..2413930c --- /dev/null +++ b/pmb2_description/ros2_control/ros2_control.urdf.xacro @@ -0,0 +1,25 @@ + + + + + + + + + gazebo_ros2_control/GazeboSystem + + + + + + + + diff --git a/pmb2_description/urdf/wheels/wheel.ros2_control.xacro b/pmb2_description/urdf/wheels/wheel.ros2_control.xacro index 06dc1921..77da69f8 100644 --- a/pmb2_description/urdf/wheels/wheel.ros2_control.xacro +++ b/pmb2_description/urdf/wheels/wheel.ros2_control.xacro @@ -1,6 +1,6 @@ - - - - - gazebo_ros2_control/GazeboSystem - + + + + -1 + 1 + + + + + + transmission_interface/SimpleTransmission + + 1.0 + - - -1 - 1 - - + hardware_interface/VelocityJointInterface + hardware_interface/JointStateInterface - - transmission_interface/SimpleTransmission - - 1.0 - - - hardware_interface/VelocityJointInterface - hardware_interface/JointStateInterface - - - + diff --git a/pmb2_description/urdf/wheels/wheel.urdf.xacro b/pmb2_description/urdf/wheels/wheel.urdf.xacro index 66f58e1c..e7a23b7d 100644 --- a/pmb2_description/urdf/wheels/wheel.urdf.xacro +++ b/pmb2_description/urdf/wheels/wheel.urdf.xacro @@ -13,7 +13,6 @@ Creative Commons, 444 Castro Street, Suite 900, Mountain View, California, 94041 - @@ -38,7 +37,7 @@ Creative Commons, 444 Castro Street, Suite 900, Mountain View, California, 94041 - + @@ -66,7 +65,7 @@ Creative Commons, 444 Castro Street, Suite 900, Mountain View, California, 94041 soft_lower_limit="-0.005" soft_upper_limit="0.005" /> - + @@ -76,9 +75,6 @@ Creative Commons, 444 Castro Street, Suite 900, Mountain View, California, 94041 - - - From 01eff97ad71ab9a81016e2f1dc742238ec20a471 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Mon, 15 Feb 2021 13:41:45 +0100 Subject: [PATCH 13/57] Migrate pmb2_bringup to ROS2 --- pmb2_bringup/CMakeLists.txt | 14 +-- pmb2_bringup/COLCON_IGNORE | 0 pmb2_bringup/config/joy_teleop.yaml | 98 ++++++++++--------- pmb2_bringup/config/twist_mux/joystick.yaml | 20 ++-- .../config/twist_mux/twist_mux_locks.yaml | 33 +++---- .../config/twist_mux/twist_mux_topics.yaml | 80 +++++++-------- pmb2_bringup/launch/joystick_teleop.launch.py | 46 +++++++++ pmb2_bringup/launch/pmb2_bringup.launch.py | 27 +++++ pmb2_bringup/launch/twist_mux.launch.py | 35 +++++++ pmb2_bringup/package.xml | 14 +-- 10 files changed, 241 insertions(+), 126 deletions(-) delete mode 100644 pmb2_bringup/COLCON_IGNORE create mode 100644 pmb2_bringup/launch/joystick_teleop.launch.py create mode 100644 pmb2_bringup/launch/pmb2_bringup.launch.py create mode 100644 pmb2_bringup/launch/twist_mux.launch.py diff --git a/pmb2_bringup/CMakeLists.txt b/pmb2_bringup/CMakeLists.txt index b0083637..78c34c5a 100644 --- a/pmb2_bringup/CMakeLists.txt +++ b/pmb2_bringup/CMakeLists.txt @@ -1,11 +1,11 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(pmb2_bringup) -find_package(catkin REQUIRED) +find_package(ament_cmake_auto REQUIRED) -catkin_package() +# PAL Robotics stricter build flags, other developers should not worry about this +find_package(ament_cmake_pal QUIET) -foreach(dir config launch) - install(DIRECTORY ${dir}/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir}) -endforeach() +ament_auto_find_build_dependencies() + +ament_auto_package(INSTALL_TO_SHARE config launch) diff --git a/pmb2_bringup/COLCON_IGNORE b/pmb2_bringup/COLCON_IGNORE deleted file mode 100644 index e69de29b..00000000 diff --git a/pmb2_bringup/config/joy_teleop.yaml b/pmb2_bringup/config/joy_teleop.yaml index 0189d9f3..953081c9 100644 --- a/pmb2_bringup/config/joy_teleop.yaml +++ b/pmb2_bringup/config/joy_teleop.yaml @@ -1,52 +1,60 @@ -teleop: - move: - type: topic - message_type: geometry_msgs/Twist - topic_name: cmd_vel - axis_mappings: - - - axis: 1 - target: linear.x - scale: 1.0 - - - axis: 2 - target: angular.z - scale: 1.0 +/**: + ros__parameters: + move: + type: topic + interface_type: geometry_msgs/msg/Twist + topic_name: cmd_vel + deadman_buttons: [] + axis_mappings: + linear-x: + axis: 1 + scale: 1.0 + offset: 0.0 + angular-z: + axis: 2 + scale: 1.0 + offset: 0.0 - joy_priority: - type: action - action_name: joy_priority_action - buttons: [0] + joy_priority: + type: action + interface_type: teleop_tools_msgs/action/Increment + action_name: joy_priority_action + buttons: [0] - joy_turbo_decrease: - type: action - action_name: joy_turbo_decrease - buttons: [1, 4, 5] + joy_turbo_decrease: + type: action + interface_type: teleop_tools_msgs/action/Increment + action_name: joy_turbo_decrease + buttons: [1, 4, 5] - joy_turbo_increase: - type: action - action_name: joy_turbo_increase - buttons: [3, 4, 5] + joy_turbo_increase: + type: action + interface_type: teleop_tools_msgs/action/Increment + action_name: joy_turbo_increase + buttons: [3, 4, 5] - joy_turbo_angular_decrease: - type: action - action_name: joy_turbo_angular_decrease - buttons: [0, 4, 5] + joy_turbo_angular_decrease: + type: action + interface_type: teleop_tools_msgs/action/Increment + action_name: joy_turbo_angular_decrease + buttons: [0, 4, 5] - joy_turbo_angular_increase: - type: action - action_name: joy_turbo_angular_increase - buttons: [2, 4, 5] + joy_turbo_angular_increase: + type: action + interface_type: teleop_tools_msgs/action/Increment + action_name: joy_turbo_angular_increase + buttons: [2, 4, 5] - joy_turbo_reset: - type: action - action_name: joy_turbo_reset - buttons: [4, 5, 7] + joy_turbo_reset: + type: action + interface_type: teleop_tools_msgs/action/Increment + action_name: joy_turbo_reset + buttons: [4, 5, 7] - # Disable speed_limit example - # @todo duration param is not taken - #speed_limit_disable: - # type: action - # action_name: speed_limit/disable - # duration: 10.0 - # buttons: [4, 5, 6, 7] + # Disable speed_limit example + # @todo duration param is not taken + #speed_limit_disable: + # type: action + # action_name: speed_limit/disable + # duration: 10.0 + # buttons: [4, 5, 6, 7] diff --git a/pmb2_bringup/config/twist_mux/joystick.yaml b/pmb2_bringup/config/twist_mux/joystick.yaml index 5eb29d7d..8912457c 100644 --- a/pmb2_bringup/config/twist_mux/joystick.yaml +++ b/pmb2_bringup/config/twist_mux/joystick.yaml @@ -1,10 +1,12 @@ -priority: false +/**: + ros__parameters: + priority: false -turbo: - linear_forward_min : 0.05 - linear_forward_max : 1.00 - linear_backward_min : 0.05 - linear_backward_max : 0.20 - angular_min : !degrees 10.0 - angular_max : !degrees 120.0 - steps : 8 + turbo: + linear_forward_min : 0.05 + linear_forward_max : 1.00 + linear_backward_min : 0.05 + linear_backward_max : 0.20 + angular_min : !degrees 10.0 + angular_max : !degrees 120.0 + steps : 8 diff --git a/pmb2_bringup/config/twist_mux/twist_mux_locks.yaml b/pmb2_bringup/config/twist_mux/twist_mux_locks.yaml index 74c45113..4f9f6bf5 100644 --- a/pmb2_bringup/config/twist_mux/twist_mux_locks.yaml +++ b/pmb2_bringup/config/twist_mux/twist_mux_locks.yaml @@ -8,21 +8,20 @@ # - priority: priority in the range [0, 255], so all the topics with priority lower than it # will be stopped/disabled -locks: -- - name : pause - topic : pause_navigation - timeout : 0.0 - # Same priority as joystick control, so it'll not block it. - priority: 100 -- - name : loop_closure - topic : stop_closing_loop - timeout : 0.0 - priority: 200 -- - name : joystick - topic : joy_priority - timeout : 0.0 - priority: 100 +/**: + ros__parameters: + locks: + pause: + topic : pause_navigation + timeout : 0.0 + # Same priority as joystick control, so it'll not block it. + priority: 100 + loop_closure: + topic : stop_closing_loop + timeout : 0.0 + priority: 200 + joystick: + topic : joy_priority + timeout : 0.0 + priority: 100 diff --git a/pmb2_bringup/config/twist_mux/twist_mux_topics.yaml b/pmb2_bringup/config/twist_mux/twist_mux_topics.yaml index a310a69d..598cdd39 100644 --- a/pmb2_bringup/config/twist_mux/twist_mux_topics.yaml +++ b/pmb2_bringup/config/twist_mux/twist_mux_topics.yaml @@ -4,45 +4,41 @@ # - topic : input topic of geometry_msgs::Twist type # - timeout : timeout in seconds to start discarding old messages, and use 0.0 speed instead # - priority: priority in the range [0, 255]; the higher the more priority over other topics - -topics: -- - name : navigation - topic : nav_vel - timeout : 0.5 - priority: 10 -- - name : joystick - topic : joy_vel - timeout : 0.5 - priority: 100 -- - name : keyboard - topic : key_vel - timeout : 0.5 - priority: 90 -- - name : tablet - topic : tab_vel - timeout : 0.5 - priority: 100 -- - name : marker - topic : marker_vel - timeout : 0.5 - priority: 99 -- - name : phone - topic : phone_vel - timeout : 0.5 - priority: 98 -- - name : rviz_joy - topic : rviz_joy_vel - timeout : 0.5 - priority: 95 -- - name : servoing_cmd_vel - topic : servoing_cmd_vel - timeout : 0.5 - priority: 20 \ No newline at end of file +/**: + ros__parameters: + topics: + navigation: + topic : nav_vel + timeout : 0.5 + priority: 10 + joystick: + topic : joy_vel + timeout : 0.5 + priority: 100 + keyboard: + topic : key_vel + timeout : 0.5 + priority: 90 + tablet: + topic : tab_vel + timeout : 0.5 + priority: 100 + marker: + name : marker + topic : marker_vel + timeout : 0.5 + priority: 99 + phone: + topic : phone_vel + timeout : 0.5 + priority: 98 + rviz: + name : rviz_joy + topic : rviz_joy_vel + timeout : 0.5 + priority: 95 + servoing: + name : servoing_cmd_vel + topic : servoing_cmd_vel + timeout : 0.5 + priority: 20 diff --git a/pmb2_bringup/launch/joystick_teleop.launch.py b/pmb2_bringup/launch/joystick_teleop.launch.py new file mode 100644 index 00000000..fc3c70a3 --- /dev/null +++ b/pmb2_bringup/launch/joystick_teleop.launch.py @@ -0,0 +1,46 @@ +# Copyright (c) 2021 PAL Robotics S.L. +# +# 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. + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +from launch_pal.include_utils import include_launch_py_description + +import os + +def generate_launch_description(): + pkg_dir = get_package_share_directory('pmb2_bringup') + joy_teleop_path = os.path.join(pkg_dir, 'config', 'joy_teleop.yaml') + + declare_cmd_vel = DeclareLaunchArgument('cmd_vel', default_value='input_joy/cmd_vel', + description='Joystick cmd_vel topic') + declare_teleop_config = DeclareLaunchArgument('teleop_config', + default_value=joy_teleop_path, + description='Joystick teleop configuration file') + joy_teleop_node = Node( + package='joy_teleop', node_executable='joy_teleop', + parameters=[LaunchConfiguration('teleop_config')], + remappings=[('cmd_vel', LaunchConfiguration('cmd_vel'))]) + + ld = LaunchDescription([ + declare_cmd_vel, + declare_teleop_config, + joy_teleop_node, + ]) + + return ld diff --git a/pmb2_bringup/launch/pmb2_bringup.launch.py b/pmb2_bringup/launch/pmb2_bringup.launch.py new file mode 100644 index 00000000..ab94ac1a --- /dev/null +++ b/pmb2_bringup/launch/pmb2_bringup.launch.py @@ -0,0 +1,27 @@ +# Copyright (c) 2021 PAL Robotics S.L. +# +# 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. + +from launch import LaunchDescription +from launch_pal.include_utils import include_launch_py_description + +def generate_launch_description(): + # Create the launch description and populate + ld = LaunchDescription([ + # TODO missing equivalent to ROS1 robot_pose_node + # TODO missing equivalent to tf_lookup, but it may have been legacy from tf1 + include_launch_py_description('pmb2_bringup', ['launch', 'twist_mux.launch.py']), + include_launch_py_description('pmb2_bringup', ['launch', 'joystick_teleop.launch.py']), + ]) + + return ld diff --git a/pmb2_bringup/launch/twist_mux.launch.py b/pmb2_bringup/launch/twist_mux.launch.py new file mode 100644 index 00000000..c7e84c17 --- /dev/null +++ b/pmb2_bringup/launch/twist_mux.launch.py @@ -0,0 +1,35 @@ +# Copyright (c) 2021 PAL Robotics S.L. +# +# 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. + +from ament_index_python.packages import get_package_share_directory +from launch_pal.include_utils import include_launch_py_description + +from launch import LaunchDescription + +import os + +def generate_launch_description(): + pkg = get_package_share_directory('pmb2_bringup') + + ld = LaunchDescription([ + include_launch_py_description('twist_mux', ['launch', 'twist_mux_launch.py'], + launch_arguments={'cmd_vel_out': 'mobile_base_controller/cmd_vel', + 'config_locks': os.path.join(pkg, 'config', 'twist_mux', 'twist_mux_locks.yaml'), + 'config_topics': os.path.join(pkg, 'config', 'twist_mux', 'twist_mux_topics.yaml'), + 'joystick': os.path.join(pkg, 'config', 'twist_mux', 'joystick.yaml'), + }.items()), + include_launch_py_description('pmb2_bringup', ['launch', 'joystick_teleop.launch.py']), + ]) + + return ld diff --git a/pmb2_bringup/package.xml b/pmb2_bringup/package.xml index 332e2f72..3c7aacb1 100644 --- a/pmb2_bringup/package.xml +++ b/pmb2_bringup/package.xml @@ -1,23 +1,25 @@ - + pmb2_bringup 3.0.14 Launch files and scripts needed to bring up the ROS nodes of a PMB2 robot. - Procopio Stein + Victor Lopez Apache License 2.0 Bence Magyar Enrique Fernandez - catkin + ament_cmake + launch_pal pmb2_controller_configuration pmb2_description robot_state_publisher - robot_pose - tf_lookup - speed_limit_node twist_mux joy joy_teleop + + ament_cmake + + From 124036de508bc09408ee3c7092a45836d219ed40 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Mon, 15 Feb 2021 13:42:40 +0100 Subject: [PATCH 14/57] Remove gazebo laser plugin namespace --- pmb2_description/urdf/sensors/range.gazebo.xacro | 2 +- pmb2_description/urdf/sensors/sick_tim571_laser.gazebo.xacro | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/pmb2_description/urdf/sensors/range.gazebo.xacro b/pmb2_description/urdf/sensors/range.gazebo.xacro index e897768a..26dbf212 100644 --- a/pmb2_description/urdf/sensors/range.gazebo.xacro +++ b/pmb2_description/urdf/sensors/range.gazebo.xacro @@ -43,7 +43,7 @@ 0.005 - + distance ~/out:=${ros_topic} diff --git a/pmb2_description/urdf/sensors/sick_tim571_laser.gazebo.xacro b/pmb2_description/urdf/sensors/sick_tim571_laser.gazebo.xacro index 6e89a2f6..b5acc6ca 100644 --- a/pmb2_description/urdf/sensors/sick_tim571_laser.gazebo.xacro +++ b/pmb2_description/urdf/sensors/sick_tim571_laser.gazebo.xacro @@ -42,7 +42,6 @@ - lidar ~/out:=${ros_topic} sensor_msgs/LaserScan From c8f70bb02e19aa4fb1f1cb2f6d6e27f349b1319a Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Mon, 15 Feb 2021 13:49:56 +0100 Subject: [PATCH 15/57] Use unstamped topic in mobile_base_controller Signed-off-by: Victor Lopez --- pmb2_bringup/launch/twist_mux.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pmb2_bringup/launch/twist_mux.launch.py b/pmb2_bringup/launch/twist_mux.launch.py index c7e84c17..eaf6bc89 100644 --- a/pmb2_bringup/launch/twist_mux.launch.py +++ b/pmb2_bringup/launch/twist_mux.launch.py @@ -24,7 +24,7 @@ def generate_launch_description(): ld = LaunchDescription([ include_launch_py_description('twist_mux', ['launch', 'twist_mux_launch.py'], - launch_arguments={'cmd_vel_out': 'mobile_base_controller/cmd_vel', + launch_arguments={'cmd_vel_out': 'mobile_base_controller/cmd_vel_unstamped', 'config_locks': os.path.join(pkg, 'config', 'twist_mux', 'twist_mux_locks.yaml'), 'config_topics': os.path.join(pkg, 'config', 'twist_mux', 'twist_mux_topics.yaml'), 'joystick': os.path.join(pkg, 'config', 'twist_mux', 'joystick.yaml'), From f66ecef44bc89f17d1781e93892d8c0617c09c88 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Mon, 15 Feb 2021 14:26:20 +0100 Subject: [PATCH 16/57] Remove old joystick_teleop.launch --- pmb2_bringup/launch/joystick_teleop.launch | 15 --------------- pmb2_bringup/launch/joystick_teleop.launch.py | 2 ++ 2 files changed, 2 insertions(+), 15 deletions(-) delete mode 100644 pmb2_bringup/launch/joystick_teleop.launch diff --git a/pmb2_bringup/launch/joystick_teleop.launch b/pmb2_bringup/launch/joystick_teleop.launch deleted file mode 100644 index 34c33363..00000000 --- a/pmb2_bringup/launch/joystick_teleop.launch +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/pmb2_bringup/launch/joystick_teleop.launch.py b/pmb2_bringup/launch/joystick_teleop.launch.py index fc3c70a3..28cbd50d 100644 --- a/pmb2_bringup/launch/joystick_teleop.launch.py +++ b/pmb2_bringup/launch/joystick_teleop.launch.py @@ -37,6 +37,8 @@ def generate_launch_description(): parameters=[LaunchConfiguration('teleop_config')], remappings=[('cmd_vel', LaunchConfiguration('cmd_vel'))]) + # Missing joy_node (not ported to ROS2 yet) + ld = LaunchDescription([ declare_cmd_vel, declare_teleop_config, From 896fd766fd4fccfcf45ad8b8ed9b3b0d7f91e0f3 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Mon, 15 Feb 2021 15:09:17 +0100 Subject: [PATCH 17/57] use_sim_time in controllers and cleanup --- .../config/joint_state_controller.yaml | 1 + .../config/mobile_base_controller.yaml | 2 ++ .../launch/default_controllers.launch | 19 -------------- .../launch/default_controllers.launch.py | 25 +++++++------------ 4 files changed, 12 insertions(+), 35 deletions(-) delete mode 100644 pmb2_controller_configuration/launch/default_controllers.launch diff --git a/pmb2_controller_configuration/config/joint_state_controller.yaml b/pmb2_controller_configuration/config/joint_state_controller.yaml index f6666b7d..65066954 100644 --- a/pmb2_controller_configuration/config/joint_state_controller.yaml +++ b/pmb2_controller_configuration/config/joint_state_controller.yaml @@ -1,6 +1,7 @@ joint_state_controller: ros__parameters: type: joint_state_controller/JointStateController + use_sim_time: True publish_rate: 50.0 diff --git a/pmb2_controller_configuration/config/mobile_base_controller.yaml b/pmb2_controller_configuration/config/mobile_base_controller.yaml index c5fd6ca7..3aaed767 100644 --- a/pmb2_controller_configuration/config/mobile_base_controller.yaml +++ b/pmb2_controller_configuration/config/mobile_base_controller.yaml @@ -1,6 +1,8 @@ mobile_base_controller: ros__parameters: + use_sim_time: True type: diff_drive_controller/DiffDriveController + use_stamped_vel: False left_wheel_names : ['wheel_left_joint'] right_wheel_names : ['wheel_right_joint'] #wheels_per_side: 1 diff --git a/pmb2_controller_configuration/launch/default_controllers.launch b/pmb2_controller_configuration/launch/default_controllers.launch deleted file mode 100644 index 7ba449a0..00000000 --- a/pmb2_controller_configuration/launch/default_controllers.launch +++ /dev/null @@ -1,19 +0,0 @@ - - - - - - - - - - - - - - diff --git a/pmb2_controller_configuration/launch/default_controllers.launch.py b/pmb2_controller_configuration/launch/default_controllers.launch.py index 23fc54a5..bf7475bb 100644 --- a/pmb2_controller_configuration/launch/default_controllers.launch.py +++ b/pmb2_controller_configuration/launch/default_controllers.launch.py @@ -24,24 +24,17 @@ # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -import os - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_pal.include_utils import include_launch_py_description def generate_launch_description(): return LaunchDescription([ - IncludeLaunchDescription( - PythonLaunchDescriptionSource([os.path.join( - get_package_share_directory('pmb2_controller_configuration'), 'launch', - 'mobile_base_controller.launch.py')]), - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource([os.path.join( - get_package_share_directory('pmb2_controller_configuration'), 'launch', - 'joint_state_controller.launch.py')]), - )]) + include_launch_py_description( + 'pmb2_controller_configuration', ['launch', 'mobile_base_controller.launch.py']), + include_launch_py_description( + 'pmb2_controller_configuration', ['launch', 'joint_state_controller.launch.py']), + # imu_sensor_controller not migrated to ROS2 yet + # include_launch_py_description( + # 'imu_sensor_controller', ['launch', 'imu_sensor_controller.launch.py']), + ]) From d4c54e733a4e9a78a4e6f4be7728be5cdb39eb19 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Mon, 15 Feb 2021 15:09:39 +0100 Subject: [PATCH 18/57] Cleanup pmb2_bringup --- .../sensor_to_cloud/bumper_to_cloud.yaml | 1 - .../sensor_to_cloud/sonar_to_cloud.yaml | 5 ---- pmb2_bringup/launch/joystick_teleop.launch.py | 2 +- pmb2_bringup/launch/pmb2_bringup.launch | 24 ------------------- pmb2_bringup/launch/twist_mux.launch | 13 ---------- 5 files changed, 1 insertion(+), 44 deletions(-) delete mode 100644 pmb2_bringup/config/sensor_to_cloud/bumper_to_cloud.yaml delete mode 100644 pmb2_bringup/config/sensor_to_cloud/sonar_to_cloud.yaml delete mode 100644 pmb2_bringup/launch/pmb2_bringup.launch delete mode 100644 pmb2_bringup/launch/twist_mux.launch diff --git a/pmb2_bringup/config/sensor_to_cloud/bumper_to_cloud.yaml b/pmb2_bringup/config/sensor_to_cloud/bumper_to_cloud.yaml deleted file mode 100644 index 561d85a1..00000000 --- a/pmb2_bringup/config/sensor_to_cloud/bumper_to_cloud.yaml +++ /dev/null @@ -1 +0,0 @@ -bumper_base_topic: bumper_base diff --git a/pmb2_bringup/config/sensor_to_cloud/sonar_to_cloud.yaml b/pmb2_bringup/config/sensor_to_cloud/sonar_to_cloud.yaml deleted file mode 100644 index 90f43809..00000000 --- a/pmb2_bringup/config/sensor_to_cloud/sonar_to_cloud.yaml +++ /dev/null @@ -1,5 +0,0 @@ -sonar_base_topic: sonar_base - -infinity_dist : 4.0 -aperture_angle: 0.18 -arc_resolution: 0.10 diff --git a/pmb2_bringup/launch/joystick_teleop.launch.py b/pmb2_bringup/launch/joystick_teleop.launch.py index 28cbd50d..53a86828 100644 --- a/pmb2_bringup/launch/joystick_teleop.launch.py +++ b/pmb2_bringup/launch/joystick_teleop.launch.py @@ -33,7 +33,7 @@ def generate_launch_description(): default_value=joy_teleop_path, description='Joystick teleop configuration file') joy_teleop_node = Node( - package='joy_teleop', node_executable='joy_teleop', + package='joy_teleop', executable='joy_teleop', parameters=[LaunchConfiguration('teleop_config')], remappings=[('cmd_vel', LaunchConfiguration('cmd_vel'))]) diff --git a/pmb2_bringup/launch/pmb2_bringup.launch b/pmb2_bringup/launch/pmb2_bringup.launch deleted file mode 100644 index 0cde6fa3..00000000 --- a/pmb2_bringup/launch/pmb2_bringup.launch +++ /dev/null @@ -1,24 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/pmb2_bringup/launch/twist_mux.launch b/pmb2_bringup/launch/twist_mux.launch deleted file mode 100644 index b11f5ffb..00000000 --- a/pmb2_bringup/launch/twist_mux.launch +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - From 5b44fe540a709eaadfccbae245eb5e3a71faa93a Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Mon, 15 Feb 2021 16:28:21 +0100 Subject: [PATCH 19/57] Add show.launch.py --- pmb2_description/launch/show.launch.py | 43 ++++++++++++++++++++++++++ pmb2_description/package.xml | 1 + pmb2_description/robots/show.launch | 9 ------ pmb2_description/robots/upload.launch | 17 ---------- 4 files changed, 44 insertions(+), 26 deletions(-) create mode 100644 pmb2_description/launch/show.launch.py delete mode 100644 pmb2_description/robots/show.launch delete mode 100644 pmb2_description/robots/upload.launch diff --git a/pmb2_description/launch/show.launch.py b/pmb2_description/launch/show.launch.py new file mode 100644 index 00000000..847d66ec --- /dev/null +++ b/pmb2_description/launch/show.launch.py @@ -0,0 +1,43 @@ +# Copyright (c) 2021 PAL Robotics S.L. +# +# 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. + +from launch import LaunchDescription +from launch_ros.actions import Node + +from launch_pal.include_utils import include_launch_py_description + + +def generate_launch_description(): + + robot_state_publisher = include_launch_py_description( + 'pmb2_description', ['launch', 'robot_state_publisher.launch.py']) + + start_joint_pub_gui = Node( + package='joint_state_publisher_gui', + executable='joint_state_publisher_gui', + name='joint_state_publisher_gui', + output='screen') + + start_rviz_cmd = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + # arguments=['-d', rviz_config_file], + output='screen') + + return LaunchDescription([ + robot_state_publisher, + start_joint_pub_gui, + start_rviz_cmd + ]) diff --git a/pmb2_description/package.xml b/pmb2_description/package.xml index 733a2a60..b96d971f 100644 --- a/pmb2_description/package.xml +++ b/pmb2_description/package.xml @@ -19,6 +19,7 @@ xacro launch launch_ros + joint_state_publisher_gui diff --git a/pmb2_description/robots/show.launch b/pmb2_description/robots/show.launch deleted file mode 100644 index d2f2edeb..00000000 --- a/pmb2_description/robots/show.launch +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - diff --git a/pmb2_description/robots/upload.launch b/pmb2_description/robots/upload.launch deleted file mode 100644 index d16f604c..00000000 --- a/pmb2_description/robots/upload.launch +++ /dev/null @@ -1,17 +0,0 @@ - - - - - - - - - - - From 0b12ae176b3d888779c4c833121c832b25ee1f37 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Mon, 15 Feb 2021 16:46:59 +0100 Subject: [PATCH 20/57] Move param utils to launch_pal --- pmb2_description/package.xml | 1 + .../pmb2_description/launch_utils.py | 192 +++++------------- 2 files changed, 47 insertions(+), 146 deletions(-) diff --git a/pmb2_description/package.xml b/pmb2_description/package.xml index b96d971f..c171e384 100644 --- a/pmb2_description/package.xml +++ b/pmb2_description/package.xml @@ -19,6 +19,7 @@ xacro launch launch_ros + launch_pal joint_state_publisher_gui diff --git a/pmb2_description/pmb2_description/launch_utils.py b/pmb2_description/pmb2_description/launch_utils.py index 4cac7397..1f71ad8e 100644 --- a/pmb2_description/pmb2_description/launch_utils.py +++ b/pmb2_description/pmb2_description/launch_utils.py @@ -24,25 +24,21 @@ from typing import Text - - def read_launch_argument(arg_name, context): return launch.utilities.perform_substitutions(context, [LaunchConfiguration(arg_name)]) class DeviceYamlParams(Substitution): - """ - Substitution that modifies the given YAML file. - Used in launch system - """ + """Substitution that modifies the given YAML file.""" def __init__(self) -> None: super().__init__() """ - Construct the substitution - :param: source_file the original YAML file to modify - """ + Construct the substitution. + + :param: source_file the original YAML file to modify + """ @property def name(self) -> List[Substitution]: diff --git a/pmb2_description/pmb2_description/pmb2_launch_utils.py b/pmb2_description/pmb2_description/pmb2_launch_utils.py index 04642ec0..7fa67dba 100644 --- a/pmb2_description/pmb2_description/pmb2_launch_utils.py +++ b/pmb2_description/pmb2_description/pmb2_launch_utils.py @@ -1,19 +1,36 @@ -from launch.actions import DeclareLaunchArgument +# Copyright (c) 2021 PAL Robotics S.L. +# +# 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. + +from launch.actions import DeclareLaunchArgument + def get_tiago_base_hw_arguments(wheel_model=False, laser_model=False, rgbd_sensors=False, default_wheel_model='moog', default_laser_model='sick-571', - default_rgbd_sensors=False #Maybe this needs to be a string + default_rgbd_sensors=False # Maybe this needs to be a string ): - """ TIAGo Base Hardware arguments helper function - Returns a list of the requested hardware LaunchArguments for tiago base - The default value can be configured passing an argument called default_NAME_OF_ARGUMENT + """ + Return TIAGo Base Hardware arguments. - example: - LaunchDescription([*get_tiago_base_hw_arguments(wheel_model=True, laser_model=True, default_laser_model='sick-571')]) + Returns a list of the requested hardware LaunchArguments for tiago base + The default value can be configured passing an argument called default_NAME_OF_ARGUMENT + example: + LaunchDescription([*get_tiago_base_hw_arguments(wheel_model=True, laser_model=True, + default_laser_model='sick-571')]) """ args = [] if wheel_model: @@ -26,13 +43,15 @@ def get_tiago_base_hw_arguments(wheel_model=False, DeclareLaunchArgument( 'laser_model', default_value=default_laser_model, - description='Base laser model. ', choices=["sick-571", "sick-561", "sick-551", "hokuyo"])) + description='Base laser model. ', + choices=["sick-571", "sick-561", "sick-551", "hokuyo"])) if rgbd_sensors: args.append( DeclareLaunchArgument( 'rgbd_sensors', default_value=default_rgbd_sensors, - description='Whether the base has RGBD sensors or not. ', choices=["True", "False"])) + description='Whether the base has RGBD sensors or not. ', + choices=["True", "False"])) return args @@ -45,15 +64,17 @@ def get_tiago_hw_arguments(arm=False, default_end_effector_model="pal-hey5", default_ft_sensor_model="schunk-ft", **kwargs): - """ TIAGo Hardware arguments helper function - Returns a list of the requested hardware LaunchArguments for tiago - The default value can be configured passing an argument called default_NAME_OF_ARGUMENT + """ + Return TIAGo Hardware arguments. - Check get_tiago_hw_arguments for more options + Returns a list of the requested hardware LaunchArguments for tiago + The default value can be configured passing an argument called default_NAME_OF_ARGUMENT - example: - LaunchDescription([*get_tiago_hw_arguments(wheel_model=True, laser_model=True, default_laser_model='sick-571')]) + Check get_tiago_hw_arguments for more options + example: + LaunchDescription([*get_tiago_hw_arguments(wheel_model=True, laser_model=True, + default_laser_model='sick-571')]) """ args = get_tiago_base_hw_arguments( rgbd_sensors=False, @@ -74,7 +95,8 @@ def get_tiago_hw_arguments(arm=False, DeclareLaunchArgument( 'end_effector_model', default_value=default_end_effector_model, - description='Wrist model. ', choices=["pal-gripper", "pal-hey5", "schunk-wsg", "custom", "False"])) + description='Wrist model.', + choices=["pal-gripper", "pal-hey5", "schunk-wsg", "custom", "False"])) if ft_sensor_model: args.append( DeclareLaunchArgument( @@ -82,4 +104,3 @@ def get_tiago_hw_arguments(arm=False, default_value=default_ft_sensor_model, description='Wrist model. ', choices=["schunk-ft", "False"])) return args - From 36c4d71d2527b4c82293ab9e8e6ff4be4a1e6bff Mon Sep 17 00:00:00 2001 From: Jordan Palacios Date: Wed, 17 Feb 2021 14:28:45 +0100 Subject: [PATCH 24/57] Fixed some tiago arguments --- .../pmb2_description/pmb2_launch_utils.py | 28 +++++++++---------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/pmb2_description/pmb2_description/pmb2_launch_utils.py b/pmb2_description/pmb2_description/pmb2_launch_utils.py index 7fa67dba..6422ed1e 100644 --- a/pmb2_description/pmb2_description/pmb2_launch_utils.py +++ b/pmb2_description/pmb2_description/pmb2_launch_utils.py @@ -58,11 +58,11 @@ def get_tiago_base_hw_arguments(wheel_model=False, def get_tiago_hw_arguments(arm=False, wrist_model=False, end_effector_model=False, - ft_sensor_model=False, - default_arm=True, - default_wrist_model="wrist-2017", + ft_sensor=False, + default_arm="True", + default_wrist_model="wrist-2010", default_end_effector_model="pal-hey5", - default_ft_sensor_model="schunk-ft", + default_ft_sensor="schunk-ft", **kwargs): """ Return TIAGo Hardware arguments. @@ -77,13 +77,13 @@ def get_tiago_hw_arguments(arm=False, default_laser_model='sick-571')]) """ args = get_tiago_base_hw_arguments( - rgbd_sensors=False, **kwargs) # RGBD on top of base are impossible if torso is installed if arm: - args.append(DeclareLaunchArgument( - 'arm', - default_value=default_arm, - description='Whether TIAGo has arm or not. ', choices=["True", "False"])) + args.append( + DeclareLaunchArgument( + 'arm', + default_value=default_arm, + description='Whether TIAGo has arm or not. ', choices=["True", "False"])) if wrist_model: args.append( DeclareLaunchArgument( @@ -95,12 +95,12 @@ def get_tiago_hw_arguments(arm=False, DeclareLaunchArgument( 'end_effector_model', default_value=default_end_effector_model, - description='Wrist model.', + description='End effector model.', choices=["pal-gripper", "pal-hey5", "schunk-wsg", "custom", "False"])) - if ft_sensor_model: + if ft_sensor: args.append( DeclareLaunchArgument( - 'ft_sensor_model', - default_value=default_ft_sensor_model, - description='Wrist model. ', choices=["schunk-ft", "False"])) + 'ft_sensor', + default_value=default_ft_sensor, + description='FT sensor model. ', choices=["schunk-ft", "False"])) return args From 21acbc38fca1ea9b3f375d1793c3b81a726f6e43 Mon Sep 17 00:00:00 2001 From: Jordan Palacios Date: Thu, 18 Feb 2021 11:21:50 +0100 Subject: [PATCH 25/57] Format --- .../pmb2_description/pmb2_launch_utils.py | 61 +++++++++++-------- 1 file changed, 35 insertions(+), 26 deletions(-) diff --git a/pmb2_description/pmb2_description/pmb2_launch_utils.py b/pmb2_description/pmb2_description/pmb2_launch_utils.py index 6422ed1e..4bf7680d 100644 --- a/pmb2_description/pmb2_description/pmb2_launch_utils.py +++ b/pmb2_description/pmb2_description/pmb2_launch_utils.py @@ -15,22 +15,24 @@ from launch.actions import DeclareLaunchArgument -def get_tiago_base_hw_arguments(wheel_model=False, - laser_model=False, - rgbd_sensors=False, - default_wheel_model='moog', - default_laser_model='sick-571', - default_rgbd_sensors=False # Maybe this needs to be a string - ): +def get_tiago_base_hw_arguments( + wheel_model=False, + laser_model=False, + rgbd_sensors=False, + default_wheel_model="moog", + default_laser_model="sick-571", + default_rgbd_sensors="False"): """ Return TIAGo Base Hardware arguments. Returns a list of the requested hardware LaunchArguments for tiago base - The default value can be configured passing an argument called default_NAME_OF_ARGUMENT + The default value can be configured passing an argument called default + NAME_OF_ARGUMENT example: - LaunchDescription([*get_tiago_base_hw_arguments(wheel_model=True, laser_model=True, - default_laser_model='sick-571')]) + LaunchDescription([*get_tiago_base_hw_arguments( + wheel_model=True, laser_model=True, + default_laser_model='sick-571')]) """ args = [] if wheel_model: @@ -55,26 +57,29 @@ def get_tiago_base_hw_arguments(wheel_model=False, return args -def get_tiago_hw_arguments(arm=False, - wrist_model=False, - end_effector_model=False, - ft_sensor=False, - default_arm="True", - default_wrist_model="wrist-2010", - default_end_effector_model="pal-hey5", - default_ft_sensor="schunk-ft", - **kwargs): +def get_tiago_hw_arguments( + arm=False, + wrist_model=False, + end_effector_model=False, + ft_sensor=False, + default_arm="True", + default_wrist_model="wrist-2010", + default_end_effector_model="pal-hey5", + default_ft_sensor="schunk-ft", + **kwargs): """ Return TIAGo Hardware arguments. Returns a list of the requested hardware LaunchArguments for tiago - The default value can be configured passing an argument called default_NAME_OF_ARGUMENT + The default value can be configured passing an argument called default + NAME_OF_ARGUMENT Check get_tiago_hw_arguments for more options example: - LaunchDescription([*get_tiago_hw_arguments(wheel_model=True, laser_model=True, - default_laser_model='sick-571')]) + LaunchDescription([*get_tiago_hw_arguments( + wheel_model=True, laser_model=True, + default_laser_model='sick-571')]) """ args = get_tiago_base_hw_arguments( **kwargs) # RGBD on top of base are impossible if torso is installed @@ -83,24 +88,28 @@ def get_tiago_hw_arguments(arm=False, DeclareLaunchArgument( 'arm', default_value=default_arm, - description='Whether TIAGo has arm or not. ', choices=["True", "False"])) + description='Whether TIAGo has arm or not. ', + choices=["True", "False"])) if wrist_model: args.append( DeclareLaunchArgument( 'wrist_model', default_value=default_wrist_model, - description='Wrist model. ', choices=["wrist-2010", "wrist-2017"])) + description='Wrist model. ', + choices=["wrist-2010", "wrist-2017"])) if end_effector_model: args.append( DeclareLaunchArgument( 'end_effector_model', default_value=default_end_effector_model, description='End effector model.', - choices=["pal-gripper", "pal-hey5", "schunk-wsg", "custom", "False"])) + choices=["pal-gripper", "pal-hey5", "schunk-wsg", + "custom", "False"])) if ft_sensor: args.append( DeclareLaunchArgument( 'ft_sensor', default_value=default_ft_sensor, - description='FT sensor model. ', choices=["schunk-ft", "False"])) + description='FT sensor model. ', + choices=["schunk-ft", "False"])) return args From 7c01df6d5bcbdeaa2efa0299a170a86995184d81 Mon Sep 17 00:00:00 2001 From: Jordan Palacios Date: Thu, 18 Feb 2021 16:35:02 +0100 Subject: [PATCH 26/57] Renamed end_effector argument --- pmb2_description/pmb2_description/pmb2_launch_utils.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/pmb2_description/pmb2_description/pmb2_launch_utils.py b/pmb2_description/pmb2_description/pmb2_launch_utils.py index 4bf7680d..4d3977e6 100644 --- a/pmb2_description/pmb2_description/pmb2_launch_utils.py +++ b/pmb2_description/pmb2_description/pmb2_launch_utils.py @@ -60,11 +60,11 @@ def get_tiago_base_hw_arguments( def get_tiago_hw_arguments( arm=False, wrist_model=False, - end_effector_model=False, + end_effector=False, ft_sensor=False, default_arm="True", default_wrist_model="wrist-2010", - default_end_effector_model="pal-hey5", + default_end_effector="pal-hey5", default_ft_sensor="schunk-ft", **kwargs): """ @@ -97,11 +97,11 @@ def get_tiago_hw_arguments( default_value=default_wrist_model, description='Wrist model. ', choices=["wrist-2010", "wrist-2017"])) - if end_effector_model: + if end_effector: args.append( DeclareLaunchArgument( - 'end_effector_model', - default_value=default_end_effector_model, + 'end_effector', + default_value=default_end_effector, description='End effector model.', choices=["pal-gripper", "pal-hey5", "schunk-wsg", "custom", "False"])) From 1d6b67e582d5d2cfb0db2e665fb192dcc2ba9374 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Wed, 3 Mar 2021 16:05:40 +0000 Subject: [PATCH 27/57] Adapt to proper parameter naming --- .../config/joint_state_controller.yaml | 2 +- .../config/mobile_base_controller.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/pmb2_controller_configuration/config/joint_state_controller.yaml b/pmb2_controller_configuration/config/joint_state_controller.yaml index 65066954..a73e04c9 100644 --- a/pmb2_controller_configuration/config/joint_state_controller.yaml +++ b/pmb2_controller_configuration/config/joint_state_controller.yaml @@ -1,4 +1,4 @@ -joint_state_controller: +/joint_state_controller: ros__parameters: type: joint_state_controller/JointStateController use_sim_time: True diff --git a/pmb2_controller_configuration/config/mobile_base_controller.yaml b/pmb2_controller_configuration/config/mobile_base_controller.yaml index 3aaed767..e3d62ab2 100644 --- a/pmb2_controller_configuration/config/mobile_base_controller.yaml +++ b/pmb2_controller_configuration/config/mobile_base_controller.yaml @@ -1,4 +1,4 @@ -mobile_base_controller: +/mobile_base_controller: ros__parameters: use_sim_time: True type: diff_drive_controller/DiffDriveController From 39042d094b19b3ad0612e9d349f87474fa667e00 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Tue, 9 Mar 2021 09:34:47 +0000 Subject: [PATCH 28/57] Cleanup unused files --- pmb2_bringup/launch/twist_mux.launch.py | 1 - pmb2_robot/package.xml | 1 - 2 files changed, 2 deletions(-) diff --git a/pmb2_bringup/launch/twist_mux.launch.py b/pmb2_bringup/launch/twist_mux.launch.py index ecd89219..20d01306 100644 --- a/pmb2_bringup/launch/twist_mux.launch.py +++ b/pmb2_bringup/launch/twist_mux.launch.py @@ -32,7 +32,6 @@ def generate_launch_description(): 'config_topics': os.path.join(pkg, 'config', 'twist_mux', 'twist_mux_topics.yaml'), 'joystick': os.path.join(pkg, 'config', 'twist_mux', 'joystick.yaml'), }.items()), - include_launch_py_description('pmb2_bringup', ['launch', 'joystick_teleop.launch.py']), ]) return ld diff --git a/pmb2_robot/package.xml b/pmb2_robot/package.xml index 02949c37..c899d288 100644 --- a/pmb2_robot/package.xml +++ b/pmb2_robot/package.xml @@ -13,7 +13,6 @@ pmb2_bringup pmb2_controller_configuration - ament_cmake From 6675dbdb8896680c87ba21f588cb76018d250a9e Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Wed, 10 Mar 2021 10:23:27 +0100 Subject: [PATCH 29/57] Migrate rest of lasers to ROS2 --- .../hokuyo_urg_04lx_ug01_laser.gazebo.xacro | 17 +++++++++++------ .../urdf/sensors/sick_tim551_laser.gazebo.xacro | 17 +++++++++++------ .../urdf/sensors/sick_tim561_laser.gazebo.xacro | 17 +++++++++++------ .../urdf/sensors/sick_tim571_laser.gazebo.xacro | 8 ++++---- 4 files changed, 37 insertions(+), 22 deletions(-) diff --git a/pmb2_description/urdf/sensors/hokuyo_urg_04lx_ug01_laser.gazebo.xacro b/pmb2_description/urdf/sensors/hokuyo_urg_04lx_ug01_laser.gazebo.xacro index 44c0d689..6f3f35ce 100644 --- a/pmb2_description/urdf/sensors/hokuyo_urg_04lx_ug01_laser.gazebo.xacro +++ b/pmb2_description/urdf/sensors/hokuyo_urg_04lx_ug01_laser.gazebo.xacro @@ -34,13 +34,18 @@ 5.6 0.001 + + gaussian + 0.0 + 0.03 + - - 0.03 - true - ${update_rate} - ${ros_topic} - ${name}_link + + + ~/out:=${ros_topic} + + sensor_msgs/LaserScan + ${name}_link Gazebo/DarkGrey diff --git a/pmb2_description/urdf/sensors/sick_tim551_laser.gazebo.xacro b/pmb2_description/urdf/sensors/sick_tim551_laser.gazebo.xacro index a08a7f59..25f5ddfe 100644 --- a/pmb2_description/urdf/sensors/sick_tim551_laser.gazebo.xacro +++ b/pmb2_description/urdf/sensors/sick_tim551_laser.gazebo.xacro @@ -34,13 +34,18 @@ 10.0 0.001 + + gaussian + 0.0 + 0.06 + - - 0.06 - true - ${update_rate} - ${ros_topic} - ${name}_link + + + ~/out:=${ros_topic} + + sensor_msgs/LaserScan + ${name}_link Gazebo/DarkGrey diff --git a/pmb2_description/urdf/sensors/sick_tim561_laser.gazebo.xacro b/pmb2_description/urdf/sensors/sick_tim561_laser.gazebo.xacro index 06f3b2a6..dbab81ff 100644 --- a/pmb2_description/urdf/sensors/sick_tim561_laser.gazebo.xacro +++ b/pmb2_description/urdf/sensors/sick_tim561_laser.gazebo.xacro @@ -34,13 +34,18 @@ 10.0 0.001 + + gaussian + 0.0 + 0.06 + - - 0.06 - true - ${update_rate} - ${ros_topic} - ${name}_link + + + ~/out:=${ros_topic} + + sensor_msgs/LaserScan + ${name}_link Gazebo/DarkGrey diff --git a/pmb2_description/urdf/sensors/sick_tim571_laser.gazebo.xacro b/pmb2_description/urdf/sensors/sick_tim571_laser.gazebo.xacro index b5acc6ca..3b1e4cc3 100644 --- a/pmb2_description/urdf/sensors/sick_tim571_laser.gazebo.xacro +++ b/pmb2_description/urdf/sensors/sick_tim571_laser.gazebo.xacro @@ -8,7 +8,7 @@ To view a copy of this license, visit http://creativecommons.org/licenses/by-nc-nd/3.0/ or send a letter to Creative Commons, 444 Castro Street, Suite 900, Mountain View, California, 94041, USA. - Laser configuration according with the SICK TiM 551 product specification: + Laser configuration according with the SICK TiM 571 product specification: https://www.sick.com/media/pdf/4/44/444/dataSheet_TiM571-2050101_1075091_en.pdf --> @@ -40,12 +40,12 @@ 0.06 - + - ~/out:=${ros_topic} + ~/out:=${ros_topic} sensor_msgs/LaserScan - ${name}_link + ${name}_link Gazebo/DarkGrey From 41679771fd32f8ff680a9ba4b7a6eb6b0aa14738 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Wed, 10 Mar 2021 13:35:31 +0000 Subject: [PATCH 30/57] Add imu plugin --- pmb2_description/gazebo/gazebo.urdf.xacro | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/pmb2_description/gazebo/gazebo.urdf.xacro b/pmb2_description/gazebo/gazebo.urdf.xacro index 1a4d8d6b..6d17addd 100644 --- a/pmb2_description/gazebo/gazebo.urdf.xacro +++ b/pmb2_description/gazebo/gazebo.urdf.xacro @@ -39,11 +39,9 @@ - 1 - 1000.0 + 10.0 gaussian @@ -61,6 +59,12 @@ + + + + ~/out:=base_imu + + From 3d32a2a8b2178014a6f7a9825a64bd629639ed7e Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Wed, 17 Mar 2021 18:12:36 +0000 Subject: [PATCH 31/57] Add ROS2 control imu --- pmb2_description/gazebo/gazebo.urdf.xacro | 6 ----- .../ros2_control/ros2_control.urdf.xacro | 2 ++ pmb2_description/urdf/base/base.urdf.xacro | 1 - .../urdf/sensors/sensors.ros2_control.xacro | 25 +++++++++++++++++ .../urdf/wheels/wheel.transmission.xacro | 27 ------------------- pmb2_description/urdf/wheels/wheel.urdf.xacro | 1 - 6 files changed, 27 insertions(+), 35 deletions(-) create mode 100644 pmb2_description/urdf/sensors/sensors.ros2_control.xacro delete mode 100644 pmb2_description/urdf/wheels/wheel.transmission.xacro diff --git a/pmb2_description/gazebo/gazebo.urdf.xacro b/pmb2_description/gazebo/gazebo.urdf.xacro index 6d17addd..5c9454b2 100644 --- a/pmb2_description/gazebo/gazebo.urdf.xacro +++ b/pmb2_description/gazebo/gazebo.urdf.xacro @@ -59,12 +59,6 @@ - - - - ~/out:=base_imu - - diff --git a/pmb2_description/ros2_control/ros2_control.urdf.xacro b/pmb2_description/ros2_control/ros2_control.urdf.xacro index 2413930c..d231332c 100644 --- a/pmb2_description/ros2_control/ros2_control.urdf.xacro +++ b/pmb2_description/ros2_control/ros2_control.urdf.xacro @@ -11,6 +11,7 @@ name="pmb2" > + @@ -20,6 +21,7 @@ + diff --git a/pmb2_description/urdf/base/base.urdf.xacro b/pmb2_description/urdf/base/base.urdf.xacro index 4995c09f..f82ef5bb 100644 --- a/pmb2_description/urdf/base/base.urdf.xacro +++ b/pmb2_description/urdf/base/base.urdf.xacro @@ -97,7 +97,6 @@ - diff --git a/pmb2_description/urdf/sensors/sensors.ros2_control.xacro b/pmb2_description/urdf/sensors/sensors.ros2_control.xacro new file mode 100644 index 00000000..96b6e951 --- /dev/null +++ b/pmb2_description/urdf/sensors/sensors.ros2_control.xacro @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + diff --git a/pmb2_description/urdf/wheels/wheel.transmission.xacro b/pmb2_description/urdf/wheels/wheel.transmission.xacro deleted file mode 100644 index e0dcf427..00000000 --- a/pmb2_description/urdf/wheels/wheel.transmission.xacro +++ /dev/null @@ -1,27 +0,0 @@ - - - - - - - - transmission_interface/SimpleTransmission - - 1.0 - - - hardware_interface/VelocityJointInterface - hardware_interface/JointStateInterface - - - - - diff --git a/pmb2_description/urdf/wheels/wheel.urdf.xacro b/pmb2_description/urdf/wheels/wheel.urdf.xacro index e7a23b7d..f4b779ed 100644 --- a/pmb2_description/urdf/wheels/wheel.urdf.xacro +++ b/pmb2_description/urdf/wheels/wheel.urdf.xacro @@ -11,7 +11,6 @@ Creative Commons, 444 Castro Street, Suite 900, Mountain View, California, 94041 - From 146f0a0710ffec3da18cfd57ff19fc16a0fd9d72 Mon Sep 17 00:00:00 2001 From: Jordan Palacios Date: Thu, 8 Apr 2021 12:34:19 +0200 Subject: [PATCH 32/57] Using joint_state_broadcaster instead of controller --- .../config/joint_state_broadcaster.yaml | 7 +++++++ .../config/joint_state_controller.yaml | 7 ------- .../launch/default_controllers.launch.py | 2 +- ...troller.launch.py => joint_state_broadcaster.launch.py} | 6 +++--- 4 files changed, 11 insertions(+), 11 deletions(-) create mode 100644 pmb2_controller_configuration/config/joint_state_broadcaster.yaml delete mode 100644 pmb2_controller_configuration/config/joint_state_controller.yaml rename pmb2_controller_configuration/launch/{joint_state_controller.launch.py => joint_state_broadcaster.launch.py} (84%) diff --git a/pmb2_controller_configuration/config/joint_state_broadcaster.yaml b/pmb2_controller_configuration/config/joint_state_broadcaster.yaml new file mode 100644 index 00000000..f057e461 --- /dev/null +++ b/pmb2_controller_configuration/config/joint_state_broadcaster.yaml @@ -0,0 +1,7 @@ +/joint_state_broadcaster: + ros__parameters: + type: joint_state_broadcaster/JointStateBroadcaster + use_sim_time: True + publish_rate: 50.0 + + diff --git a/pmb2_controller_configuration/config/joint_state_controller.yaml b/pmb2_controller_configuration/config/joint_state_controller.yaml deleted file mode 100644 index a73e04c9..00000000 --- a/pmb2_controller_configuration/config/joint_state_controller.yaml +++ /dev/null @@ -1,7 +0,0 @@ -/joint_state_controller: - ros__parameters: - type: joint_state_controller/JointStateController - use_sim_time: True - publish_rate: 50.0 - - diff --git a/pmb2_controller_configuration/launch/default_controllers.launch.py b/pmb2_controller_configuration/launch/default_controllers.launch.py index 95ab0666..b358e18a 100644 --- a/pmb2_controller_configuration/launch/default_controllers.launch.py +++ b/pmb2_controller_configuration/launch/default_controllers.launch.py @@ -22,7 +22,7 @@ def generate_launch_description(): include_launch_py_description( 'pmb2_controller_configuration', ['launch', 'mobile_base_controller.launch.py']), include_launch_py_description( - 'pmb2_controller_configuration', ['launch', 'joint_state_controller.launch.py']), + 'pmb2_controller_configuration', ['launch', 'joint_state_broadcaster.launch.py']), # imu_sensor_controller not migrated to ROS2 yet # include_launch_py_description( # 'imu_sensor_controller', ['launch', 'imu_sensor_controller.launch.py']), diff --git a/pmb2_controller_configuration/launch/joint_state_controller.launch.py b/pmb2_controller_configuration/launch/joint_state_broadcaster.launch.py similarity index 84% rename from pmb2_controller_configuration/launch/joint_state_controller.launch.py rename to pmb2_controller_configuration/launch/joint_state_broadcaster.launch.py index bae89006..f742f3ef 100644 --- a/pmb2_controller_configuration/launch/joint_state_controller.launch.py +++ b/pmb2_controller_configuration/launch/joint_state_broadcaster.launch.py @@ -21,8 +21,8 @@ def generate_launch_description(): return generate_load_controller_launch_description( - controller_name='joint_state_controller', - controller_type='joint_state_controller/JointStateController', + controller_name='joint_state_broadcaster', + controller_type='joint_state_broadcaster/JointStateBroadcaster', controller_params_file=os.path.join( get_package_share_directory('pmb2_controller_configuration'), - 'config', 'joint_state_controller.yaml')) + 'config', 'joint_state_broadcaster.yaml')) From 29f18fe18bb0af47753b34fdd824e6035fbe4de6 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Fri, 28 May 2021 15:38:56 +0000 Subject: [PATCH 33/57] Correct physic property names for newer gazebo --- .../urdf/wheels/caster.gazebo.xacro | 38 +++++++++++++------ .../urdf/wheels/wheel.gazebo.xacro | 22 ++++++----- 2 files changed, 40 insertions(+), 20 deletions(-) diff --git a/pmb2_description/urdf/wheels/caster.gazebo.xacro b/pmb2_description/urdf/wheels/caster.gazebo.xacro index d75b0e04..1de6c4b5 100644 --- a/pmb2_description/urdf/wheels/caster.gazebo.xacro +++ b/pmb2_description/urdf/wheels/caster.gazebo.xacro @@ -17,30 +17,46 @@ 100000000.0 10.0 - 0.1 - 0.1 + + + 1.0 + 1.0 + + - 10.0 - 0.0005 + 10.0 + 0.0005 Gazebo/DarkGrey - 10000000.0 + 100000000.0 10.0 - 0.1 - 0.1 + + + 1.0 + 1.0 + + - 10.0 - 0.0005 + 10.0 + 0.0005 Gazebo/DarkGrey - 0 + + + 0 + + - 0 + + + 0 + + diff --git a/pmb2_description/urdf/wheels/wheel.gazebo.xacro b/pmb2_description/urdf/wheels/wheel.gazebo.xacro index c4d024a3..3efd7efa 100644 --- a/pmb2_description/urdf/wheels/wheel.gazebo.xacro +++ b/pmb2_description/urdf/wheels/wheel.gazebo.xacro @@ -16,19 +16,23 @@ 1000000.0 100.0 - - 100000000000000.0 - 100000000000000.0 + + + 1.0 + 1.0 + + - 1.0 - 0.002 + 1.0 + 0.002 Gazebo/DarkGrey - 1 + + + 1 + + From 58f23c7f1c4345de8963fc01ee67821d645227e4 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Fri, 28 May 2021 15:39:07 +0000 Subject: [PATCH 34/57] Tune a bit physics to avoid joint slippage, specially in TIAGo --- pmb2_description/urdf/wheels/caster.gazebo.xacro | 12 ++++++------ pmb2_description/urdf/wheels/caster.urdf.xacro | 3 ++- pmb2_description/urdf/wheels/wheel.urdf.xacro | 1 + 3 files changed, 9 insertions(+), 7 deletions(-) diff --git a/pmb2_description/urdf/wheels/caster.gazebo.xacro b/pmb2_description/urdf/wheels/caster.gazebo.xacro index 1de6c4b5..30d753ca 100644 --- a/pmb2_description/urdf/wheels/caster.gazebo.xacro +++ b/pmb2_description/urdf/wheels/caster.gazebo.xacro @@ -16,7 +16,7 @@ 100000000.0 - 10.0 + 100000.0 1.0 @@ -24,14 +24,14 @@ - 10.0 - 0.0005 + 1.0 + 0.005 Gazebo/DarkGrey 100000000.0 - 10.0 + 100000.0 1.0 @@ -39,8 +39,8 @@ - 10.0 - 0.0005 + 1.0 + 0.005 Gazebo/DarkGrey diff --git a/pmb2_description/urdf/wheels/caster.urdf.xacro b/pmb2_description/urdf/wheels/caster.urdf.xacro index 9a7a3675..bee44a3d 100644 --- a/pmb2_description/urdf/wheels/caster.urdf.xacro +++ b/pmb2_description/urdf/wheels/caster.urdf.xacro @@ -64,7 +64,7 @@ - + @@ -72,6 +72,7 @@ + diff --git a/pmb2_description/urdf/wheels/wheel.urdf.xacro b/pmb2_description/urdf/wheels/wheel.urdf.xacro index f4b779ed..25397fe2 100644 --- a/pmb2_description/urdf/wheels/wheel.urdf.xacro +++ b/pmb2_description/urdf/wheels/wheel.urdf.xacro @@ -72,6 +72,7 @@ Creative Commons, 444 Castro Street, Suite 900, Mountain View, California, 94041 + From 212f1dd5445a7fa7e42b27f0e51b30d53302c543 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Fri, 11 Jun 2021 10:49:39 +0000 Subject: [PATCH 35/57] No laser value is 'no-laser', change rgbd_sensors to courier_rgbd_sensors --- .../launch/robot_state_publisher.launch.py | 8 ++++---- .../pmb2_description/pmb2_launch_utils.py | 12 ++++++------ pmb2_description/robots/pmb2.urdf.xacro | 10 +++++----- 3 files changed, 15 insertions(+), 15 deletions(-) diff --git a/pmb2_description/launch/robot_state_publisher.launch.py b/pmb2_description/launch/robot_state_publisher.launch.py index e9e2d5d8..c0986471 100644 --- a/pmb2_description/launch/robot_state_publisher.launch.py +++ b/pmb2_description/launch/robot_state_publisher.launch.py @@ -41,8 +41,8 @@ def describe(self) -> Text: def perform(self, context: LaunchContext) -> Text: """Generate the robot description and return it as a string.""" laser_model = read_launch_argument("laser_model", context) - rgbd_sensors = read_launch_argument("rgbd_sensors", context) - return " laser_model:=" + laser_model + " rgbd_sensors:=" + rgbd_sensors + courier_rgbd_sensors = read_launch_argument("courier_rgbd_sensors", context) + return " laser_model:=" + laser_model + " courier_rgbd_sensors:=" + courier_rgbd_sensors def generate_launch_description(): @@ -63,8 +63,8 @@ def generate_launch_description(): parameters=[parameters]) return LaunchDescription([ *get_tiago_base_hw_arguments(laser_model=True, - rgbd_sensors=True, + courier_rgbd_sensors=True, default_laser_model="sick-571", - default_rgbd_sensors="False"), + default_courier_rgbd_sensors="False"), rsp ]) diff --git a/pmb2_description/pmb2_description/pmb2_launch_utils.py b/pmb2_description/pmb2_description/pmb2_launch_utils.py index 4d3977e6..675e56ee 100644 --- a/pmb2_description/pmb2_description/pmb2_launch_utils.py +++ b/pmb2_description/pmb2_description/pmb2_launch_utils.py @@ -18,10 +18,10 @@ def get_tiago_base_hw_arguments( wheel_model=False, laser_model=False, - rgbd_sensors=False, + courier_rgbd_sensors=False, default_wheel_model="moog", default_laser_model="sick-571", - default_rgbd_sensors="False"): + default_courier_rgbd_sensors="False"): """ Return TIAGo Base Hardware arguments. @@ -46,12 +46,12 @@ def get_tiago_base_hw_arguments( 'laser_model', default_value=default_laser_model, description='Base laser model. ', - choices=["sick-571", "sick-561", "sick-551", "hokuyo"])) - if rgbd_sensors: + choices=["no-laser", "sick-571", "sick-561", "sick-551", "hokuyo"])) + if courier_rgbd_sensors: args.append( DeclareLaunchArgument( - 'rgbd_sensors', - default_value=default_rgbd_sensors, + 'courier_rgbd_sensors', + default_value=default_courier_rgbd_sensors, description='Whether the base has RGBD sensors or not. ', choices=["True", "False"])) return args diff --git a/pmb2_description/robots/pmb2.urdf.xacro b/pmb2_description/robots/pmb2.urdf.xacro index 0a4bc112..179dd52a 100644 --- a/pmb2_description/robots/pmb2.urdf.xacro +++ b/pmb2_description/robots/pmb2.urdf.xacro @@ -11,11 +11,11 @@ - - + + - - + + @@ -39,7 +39,7 @@ - + From 8beff4114ef85073d58f95fd087dd38c725ac1f9 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Fri, 11 Jun 2021 14:18:51 +0000 Subject: [PATCH 36/57] Restructuring code and add description test --- pmb2_description/CMakeLists.txt | 18 ++- .../launch/robot_state_publisher.launch.py | 5 +- pmb2_description/package.xml | 4 +- .../pmb2_description/launch_utils.py | 103 ------------------ .../pmb2_description/pmb2_launch_utils.py | 58 ---------- .../test/test_description.launch.py | 28 +++++ pmb2_description/test/test_pmb2.test | 11 -- 7 files changed, 45 insertions(+), 182 deletions(-) delete mode 100644 pmb2_description/pmb2_description/launch_utils.py create mode 100644 pmb2_description/test/test_description.launch.py delete mode 100644 pmb2_description/test/test_pmb2.test diff --git a/pmb2_description/CMakeLists.txt b/pmb2_description/CMakeLists.txt index 6511c79e..36d5f179 100644 --- a/pmb2_description/CMakeLists.txt +++ b/pmb2_description/CMakeLists.txt @@ -16,15 +16,21 @@ ament_auto_find_build_dependencies() if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) + find_package(urdf_test REQUIRED) ament_lint_auto_find_test_dependencies() - # find_package(rostest REQUIRED) - # foreach(laser_model false hokuyo sick-551 sick-561 sick-571) - # foreach(rgbd_sensors false true) - # add_rostest(test/test_pmb2.test ARGS laser_model:=${laser_model} rgbd_sensors:=${rgbd_sensors}) - # endforeach(rgbd_sensors) - # endforeach(laser_model) + find_package(launch_testing_ament_cmake) + + foreach(laser_model no-laser hokuyo sick-551 sick-561 sick-571) + foreach(courier_rgbd_sensors False True) + add_launch_test( + test/test_description.launch.py + TARGET "pmb2_description_${laser_model}_${courier_rgbd_sensors}" + ARGS "laser_model:=${laser_model}" "courier_rgbd_sensors:=${courier_rgbd_sensors}" + ) + endforeach() + endforeach() endif() # Install Python modules diff --git a/pmb2_description/launch/robot_state_publisher.launch.py b/pmb2_description/launch/robot_state_publisher.launch.py index c0986471..b00b2e19 100644 --- a/pmb2_description/launch/robot_state_publisher.launch.py +++ b/pmb2_description/launch/robot_state_publisher.launch.py @@ -13,13 +13,13 @@ # limitations under the License. from launch import LaunchDescription, Substitution, LaunchContext +from launch_pal.arg_utils import read_launch_argument from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare, ExecutableInPackage from launch.substitutions import Command, PathJoinSubstitution from typing import List from typing import Text -from pmb2_description.launch_utils import read_launch_argument from pmb2_description.pmb2_launch_utils import get_tiago_base_hw_arguments @@ -41,7 +41,8 @@ def describe(self) -> Text: def perform(self, context: LaunchContext) -> Text: """Generate the robot description and return it as a string.""" laser_model = read_launch_argument("laser_model", context) - courier_rgbd_sensors = read_launch_argument("courier_rgbd_sensors", context) + courier_rgbd_sensors = read_launch_argument( + "courier_rgbd_sensors", context) return " laser_model:=" + laser_model + " courier_rgbd_sensors:=" + courier_rgbd_sensors diff --git a/pmb2_description/package.xml b/pmb2_description/package.xml index e007a64d..4034d091 100644 --- a/pmb2_description/package.xml +++ b/pmb2_description/package.xml @@ -24,8 +24,8 @@ ament_lint_auto ament_lint_common - + + urdf_test ament_cmake diff --git a/pmb2_description/pmb2_description/launch_utils.py b/pmb2_description/pmb2_description/launch_utils.py deleted file mode 100644 index 1f71ad8e..00000000 --- a/pmb2_description/pmb2_description/launch_utils.py +++ /dev/null @@ -1,103 +0,0 @@ -# Copyright (c) 2021 PAL Robotics S.L. -# -# 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. - -from ament_index_python.packages import get_package_share_directory -import os - -from launch import Substitution -from launch.substitutions import LaunchConfiguration -from launch_pal.param_utils import merge_param_files -import launch - -from typing import List -from typing import Text - - -def read_launch_argument(arg_name, context): - return launch.utilities.perform_substitutions(context, - [LaunchConfiguration(arg_name)]) - - -class DeviceYamlParams(Substitution): - """Substitution that modifies the given YAML file.""" - - def __init__(self) -> None: - super().__init__() - """ - Construct the substitution. - - :param: source_file the original YAML file to modify - """ - - @property - def name(self) -> List[Substitution]: - """Getter for name.""" - return 'potato name' - - def describe(self) -> Text: - """Return a description of this substitution as a string.""" - return 'potat describe' - - def get_motor_config_path(self): - return os.path.join(get_package_share_directory( - 'pal_deployer_cfg_tiago'), 'config', 'device', 'motor', '') - - def get_sensor_config_path(self): - return os.path.join(get_package_share_directory( - 'pal_deployer_cfg_tiago'), 'config', 'device', 'sensor', '') - - def get_wheel_motor_files(self, context: launch.LaunchContext): - # Read the provided variable for model name - wheel_model = read_launch_argument('wheel_model', context) - return [self.get_motor_config_path() + 'wheel_left_motor_{}.yaml'.format(wheel_model), - self.get_motor_config_path() + 'wheel_right_motor_{}.yaml'.format(wheel_model)] - - def get_device_files(self, context: launch.LaunchContext): - # Read the provided variable for model name - wheel_model = read_launch_argument('wheel_model', context) - sensors = [] - motors = ['wheel_left_motor_{}'.format(wheel_model), - 'wheel_right_motor_{}'.format(wheel_model)] - arm = read_launch_argument('arm', context) - if arm == 'True': - wrist_model = read_launch_argument('wrist_model', context) - if wrist_model == 'wrist-2010': - wrist_suffix = '_wrist_2010' - else: - wrist_suffix = '_wrist_2017' - - motors += ['arm_1_motor', 'arm_2_motor', 'arm_3_motor', 'arm_4_motor', - 'arm_5_motor' + wrist_suffix, - 'arm_6_motor' + wrist_suffix, - 'arm_7_motor' + wrist_suffix] - - ft_sensor_model = read_launch_argument('ft_sensor_model', context) - if ft_sensor_model == 'schunk-ft': - sensors = ['fts' + wrist_suffix] - full_path_list = [] - for motor in motors: - full_path_list.append( - [self.get_motor_config_path() + motor + '.yaml', 'actuators']) - - for sensor in sensors: - full_path_list.append( - [self.get_sensor_config_path() + sensor + '.yaml', 'sensors']) - - return full_path_list - - def perform(self, context: launch.LaunchContext) -> Text: - - yaml_files = [] - yaml_files += self.get_device_files(context) - return merge_param_files(yaml_files) diff --git a/pmb2_description/pmb2_description/pmb2_launch_utils.py b/pmb2_description/pmb2_description/pmb2_launch_utils.py index 675e56ee..cd942b46 100644 --- a/pmb2_description/pmb2_description/pmb2_launch_utils.py +++ b/pmb2_description/pmb2_description/pmb2_launch_utils.py @@ -55,61 +55,3 @@ def get_tiago_base_hw_arguments( description='Whether the base has RGBD sensors or not. ', choices=["True", "False"])) return args - - -def get_tiago_hw_arguments( - arm=False, - wrist_model=False, - end_effector=False, - ft_sensor=False, - default_arm="True", - default_wrist_model="wrist-2010", - default_end_effector="pal-hey5", - default_ft_sensor="schunk-ft", - **kwargs): - """ - Return TIAGo Hardware arguments. - - Returns a list of the requested hardware LaunchArguments for tiago - The default value can be configured passing an argument called default - NAME_OF_ARGUMENT - - Check get_tiago_hw_arguments for more options - - example: - LaunchDescription([*get_tiago_hw_arguments( - wheel_model=True, laser_model=True, - default_laser_model='sick-571')]) - """ - args = get_tiago_base_hw_arguments( - **kwargs) # RGBD on top of base are impossible if torso is installed - if arm: - args.append( - DeclareLaunchArgument( - 'arm', - default_value=default_arm, - description='Whether TIAGo has arm or not. ', - choices=["True", "False"])) - if wrist_model: - args.append( - DeclareLaunchArgument( - 'wrist_model', - default_value=default_wrist_model, - description='Wrist model. ', - choices=["wrist-2010", "wrist-2017"])) - if end_effector: - args.append( - DeclareLaunchArgument( - 'end_effector', - default_value=default_end_effector, - description='End effector model.', - choices=["pal-gripper", "pal-hey5", "schunk-wsg", - "custom", "False"])) - if ft_sensor: - args.append( - DeclareLaunchArgument( - 'ft_sensor', - default_value=default_ft_sensor, - description='FT sensor model. ', - choices=["schunk-ft", "False"])) - return args diff --git a/pmb2_description/test/test_description.launch.py b/pmb2_description/test/test_description.launch.py new file mode 100644 index 00000000..76dab24b --- /dev/null +++ b/pmb2_description/test/test_description.launch.py @@ -0,0 +1,28 @@ +# Copyright (c) 2021 PAL Robotics S.L. +# +# 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. + +from urdf_test.description_test import (generate_urdf_test_description, + TestDescriptionPublished, TestSuccessfulExit) +from launch_pal.include_utils import include_launch_py_description + +# Ignore unused import warnings for the Test Classes +__all__ = ('TestDescriptionPublished', 'TestSuccessfulExit') + + +def generate_test_description(): + return generate_urdf_test_description( + include_launch_py_description( + 'pmb2_description', ['launch', 'robot_state_publisher.launch.py']), + + ) diff --git a/pmb2_description/test/test_pmb2.test b/pmb2_description/test/test_pmb2.test deleted file mode 100644 index 55d7abba..00000000 --- a/pmb2_description/test/test_pmb2.test +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - - From 14787a75305e660f5bd0df04675781a57151ab52 Mon Sep 17 00:00:00 2001 From: cescfolch Date: Wed, 16 Jun 2021 10:09:36 +0000 Subject: [PATCH 37/57] add extra joints to joint state --- .../config/joint_state_broadcaster.yaml | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/pmb2_controller_configuration/config/joint_state_broadcaster.yaml b/pmb2_controller_configuration/config/joint_state_broadcaster.yaml index f057e461..68823f70 100644 --- a/pmb2_controller_configuration/config/joint_state_broadcaster.yaml +++ b/pmb2_controller_configuration/config/joint_state_broadcaster.yaml @@ -3,5 +3,4 @@ type: joint_state_broadcaster/JointStateBroadcaster use_sim_time: True publish_rate: 50.0 - - + extra_joints: ['caster_back_left_1_joint', 'caster_back_left_2_joint', 'caster_front_left_1_joint', 'caster_front_left_2_joint', 'caster_back_right_1_joint', 'caster_back_right_2_joint', 'caster_front_right_1_joint', 'caster_front_right_2_joint', 'suspension_left_joint', 'suspension_right_joint'] From a52c1d6e044d83c33985ad8a8192c11e6f80d0ce Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Wed, 30 Jun 2021 09:05:16 +0000 Subject: [PATCH 38/57] Correct dependency name --- pmb2_controller_configuration/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pmb2_controller_configuration/package.xml b/pmb2_controller_configuration/package.xml index a0241138..51f2c229 100644 --- a/pmb2_controller_configuration/package.xml +++ b/pmb2_controller_configuration/package.xml @@ -14,7 +14,7 @@ controller_manager diff_drive_controller joint_state_controller - imu_sensor_controller + imu_sensor_broadcaster ament_lint_auto ament_lint_common From 132f2d0fdf328416060d643e31d98aa136531fd9 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Mon, 12 Jul 2021 11:55:51 +0200 Subject: [PATCH 39/57] Set back old version number before release --- pmb2_description/package.xml | 2 +- pmb2_robot/package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/pmb2_description/package.xml b/pmb2_description/package.xml index 4034d091..fa17eef2 100644 --- a/pmb2_description/package.xml +++ b/pmb2_description/package.xml @@ -1,7 +1,7 @@ pmb2_description - 4.0.0 + 3.0.14 Mechanical, kinematic, visual, etc. description of the PMB2 robot. The files in this package are parsed and used by diff --git a/pmb2_robot/package.xml b/pmb2_robot/package.xml index c899d288..1fe95ded 100644 --- a/pmb2_robot/package.xml +++ b/pmb2_robot/package.xml @@ -1,7 +1,7 @@ pmb2_robot - 4.0.0 + 3.0.14 PMB2 robot description and launch files Victor Lopez Apache License 2.0 From 8231528c206c235dfba849f9729731dc19fb3840 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Mon, 12 Jul 2021 11:56:17 +0200 Subject: [PATCH 40/57] Updated Changelog --- pmb2_bringup/CHANGELOG.rst | 11 ++++++++ pmb2_controller_configuration/CHANGELOG.rst | 17 ++++++++++++ pmb2_description/CHANGELOG.rst | 29 +++++++++++++++++++++ pmb2_robot/CHANGELOG.rst | 8 ++++++ 4 files changed, 65 insertions(+) diff --git a/pmb2_bringup/CHANGELOG.rst b/pmb2_bringup/CHANGELOG.rst index ab5985ab..e19d97fb 100644 --- a/pmb2_bringup/CHANGELOG.rst +++ b/pmb2_bringup/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package pmb2_bringup ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Cleanup unused files +* Add linters and fix errors +* Cleanup pmb2_bringup +* Remove old joystick_teleop.launch +* Use unstamped topic in mobile_base_controller +* Migrate pmb2_bringup to ROS2 +* First working version +* Contributors: Victor Lopez + 3.0.14 (2021-01-18) ------------------- diff --git a/pmb2_controller_configuration/CHANGELOG.rst b/pmb2_controller_configuration/CHANGELOG.rst index 6c18f4dc..787b70bc 100644 --- a/pmb2_controller_configuration/CHANGELOG.rst +++ b/pmb2_controller_configuration/CHANGELOG.rst @@ -2,6 +2,23 @@ Changelog for package pmb2_controller_configuration ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Correct dependency name +* Using joint_state_broadcaster instead of controller +* Adapt to proper parameter naming +* Add linters to pmb2_bringup and apply fixes +* use_sim_time in controllers and cleanup +* Split default_controllers launch file +* Fixes to gazebo ros2 control param changes +* More fixes to default_controllers +* Add default_controllers.launch.py +* Update default_controllers.yaml + Update gazebo controller name +* Add pmb2_controller_configuration +* First working version +* Contributors: Jordan Palacios, Victor Lopez + 3.0.14 (2021-01-18) ------------------- diff --git a/pmb2_description/CHANGELOG.rst b/pmb2_description/CHANGELOG.rst index 5e9e8c1d..2661212a 100644 --- a/pmb2_description/CHANGELOG.rst +++ b/pmb2_description/CHANGELOG.rst @@ -2,6 +2,35 @@ Changelog for package pmb2_description ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Set back old version number before release +* Restructuring code and add description test +* No laser value is 'no-laser', change rgbd_sensors to courier_rgbd_sensors +* Tune a bit physics to avoid joint slippage, specially in TIAGo +* Correct physic property names for newer gazebo +* Add ROS2 control imu +* Add imu plugin +* Migrate rest of lasers to ROS2 +* Renamed end_effector argument +* Format +* Fixed some tiago arguments +* Add linters to pmb2_description and apply fixes +* Move param utils to launch_pal +* Add show.launch.py +* Remove gazebo laser plugin namespace +* Merge branch 'single_ros2_control_system' into 'foxy-devel' + Single ROS2 control system + See merge request robots/pmb2_robot!65 +* All joints now form part of a single ros2_control system +* Fixes to gazebo ros2 control param changes +* Update how ros2 gazebo plugin is loaded +* Add wheel ros2_control file +* First working version +* Remove comments to workaround https://github.com/ros2/launch_ros/issues/214 +* First WIP of upload.py +* Contributors: Jordan Palacios, Victor Lopez, victor + 3.0.14 (2021-01-18) ------------------- * Merge branch 'fix_wheel_slippage' into 'erbium-devel' diff --git a/pmb2_robot/CHANGELOG.rst b/pmb2_robot/CHANGELOG.rst index 831899d0..347b2cd7 100644 --- a/pmb2_robot/CHANGELOG.rst +++ b/pmb2_robot/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package pmb2_robot ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Set back old version number before release +* Cleanup unused files +* First working version +* First WIP of upload.py +* Contributors: Victor Lopez + 3.0.14 (2021-01-18) ------------------- From 36bcffd9471f8ecf2a51f17aa0adae53747ca553 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Mon, 12 Jul 2021 11:56:19 +0200 Subject: [PATCH 41/57] 4.0.0 --- pmb2_bringup/CHANGELOG.rst | 4 ++-- pmb2_bringup/package.xml | 2 +- pmb2_controller_configuration/CHANGELOG.rst | 4 ++-- pmb2_controller_configuration/package.xml | 2 +- pmb2_description/CHANGELOG.rst | 4 ++-- pmb2_description/package.xml | 2 +- pmb2_robot/CHANGELOG.rst | 4 ++-- pmb2_robot/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/pmb2_bringup/CHANGELOG.rst b/pmb2_bringup/CHANGELOG.rst index e19d97fb..573affc6 100644 --- a/pmb2_bringup/CHANGELOG.rst +++ b/pmb2_bringup/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pmb2_bringup ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2021-07-12) +------------------ * Cleanup unused files * Add linters and fix errors * Cleanup pmb2_bringup diff --git a/pmb2_bringup/package.xml b/pmb2_bringup/package.xml index 965aa50e..b6c182eb 100644 --- a/pmb2_bringup/package.xml +++ b/pmb2_bringup/package.xml @@ -1,7 +1,7 @@ pmb2_bringup - 3.0.14 + 4.0.0 Launch files and scripts needed to bring up the ROS nodes of a PMB2 robot. Victor Lopez Apache License 2.0 diff --git a/pmb2_controller_configuration/CHANGELOG.rst b/pmb2_controller_configuration/CHANGELOG.rst index 787b70bc..e26b8e5b 100644 --- a/pmb2_controller_configuration/CHANGELOG.rst +++ b/pmb2_controller_configuration/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pmb2_controller_configuration ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2021-07-12) +------------------ * Correct dependency name * Using joint_state_broadcaster instead of controller * Adapt to proper parameter naming diff --git a/pmb2_controller_configuration/package.xml b/pmb2_controller_configuration/package.xml index 51f2c229..5857dcf7 100644 --- a/pmb2_controller_configuration/package.xml +++ b/pmb2_controller_configuration/package.xml @@ -1,7 +1,7 @@ pmb2_controller_configuration - 3.0.14 + 4.0.0 Launch files and scripts needed to configure the controllers of the PMB2 robot. Victor Lopez diff --git a/pmb2_description/CHANGELOG.rst b/pmb2_description/CHANGELOG.rst index 2661212a..c7d5aa7b 100644 --- a/pmb2_description/CHANGELOG.rst +++ b/pmb2_description/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pmb2_description ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2021-07-12) +------------------ * Set back old version number before release * Restructuring code and add description test * No laser value is 'no-laser', change rgbd_sensors to courier_rgbd_sensors diff --git a/pmb2_description/package.xml b/pmb2_description/package.xml index fa17eef2..4034d091 100644 --- a/pmb2_description/package.xml +++ b/pmb2_description/package.xml @@ -1,7 +1,7 @@ pmb2_description - 3.0.14 + 4.0.0 Mechanical, kinematic, visual, etc. description of the PMB2 robot. The files in this package are parsed and used by diff --git a/pmb2_robot/CHANGELOG.rst b/pmb2_robot/CHANGELOG.rst index 347b2cd7..a5dc5b84 100644 --- a/pmb2_robot/CHANGELOG.rst +++ b/pmb2_robot/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pmb2_robot ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2021-07-12) +------------------ * Set back old version number before release * Cleanup unused files * First working version diff --git a/pmb2_robot/package.xml b/pmb2_robot/package.xml index 1fe95ded..c899d288 100644 --- a/pmb2_robot/package.xml +++ b/pmb2_robot/package.xml @@ -1,7 +1,7 @@ pmb2_robot - 3.0.14 + 4.0.0 PMB2 robot description and launch files Victor Lopez Apache License 2.0 From 93ac4f0d1c84c9017cbba3524f35c836779343a4 Mon Sep 17 00:00:00 2001 From: victor Date: Tue, 13 Jul 2021 08:51:34 +0000 Subject: [PATCH 42/57] Add license --- LICENSE | 201 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 201 insertions(+) create mode 100644 LICENSE diff --git a/LICENSE b/LICENSE new file mode 100644 index 00000000..339a484e --- /dev/null +++ b/LICENSE @@ -0,0 +1,201 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright 2021 Robots + + 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. From a0fb186ab7d2b009c321a631cf144c1a2598995a Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Thu, 15 Jul 2021 11:57:49 +0200 Subject: [PATCH 43/57] Fix missing dependencies --- pmb2_bringup/package.xml | 2 +- pmb2_controller_configuration/package.xml | 2 +- pmb2_description/package.xml | 1 + 3 files changed, 3 insertions(+), 2 deletions(-) diff --git a/pmb2_bringup/package.xml b/pmb2_bringup/package.xml index b6c182eb..f6daf181 100644 --- a/pmb2_bringup/package.xml +++ b/pmb2_bringup/package.xml @@ -8,7 +8,7 @@ Bence Magyar Enrique Fernandez - ament_cmake + ament_cmake_auto launch_pal pmb2_controller_configuration diff --git a/pmb2_controller_configuration/package.xml b/pmb2_controller_configuration/package.xml index 5857dcf7..a0234583 100644 --- a/pmb2_controller_configuration/package.xml +++ b/pmb2_controller_configuration/package.xml @@ -9,7 +9,7 @@ Apache License 2.0 Enrique Fernandez - ament_cmake + ament_cmake_auto controller_manager diff_drive_controller diff --git a/pmb2_description/package.xml b/pmb2_description/package.xml index 4034d091..a6d4ba91 100644 --- a/pmb2_description/package.xml +++ b/pmb2_description/package.xml @@ -24,6 +24,7 @@ ament_lint_auto ament_lint_common + launch_testing_ament urdf_test From d62d66a38b00f701a1ed7a81b34b45d19e720950 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Thu, 15 Jul 2021 11:58:01 +0200 Subject: [PATCH 44/57] Updated Changelog --- pmb2_bringup/CHANGELOG.rst | 5 +++++ pmb2_controller_configuration/CHANGELOG.rst | 5 +++++ pmb2_description/CHANGELOG.rst | 5 +++++ pmb2_robot/CHANGELOG.rst | 3 +++ 4 files changed, 18 insertions(+) diff --git a/pmb2_bringup/CHANGELOG.rst b/pmb2_bringup/CHANGELOG.rst index 573affc6..8906e1b0 100644 --- a/pmb2_bringup/CHANGELOG.rst +++ b/pmb2_bringup/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package pmb2_bringup ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix missing dependencies +* Contributors: Victor Lopez + 4.0.0 (2021-07-12) ------------------ * Cleanup unused files diff --git a/pmb2_controller_configuration/CHANGELOG.rst b/pmb2_controller_configuration/CHANGELOG.rst index e26b8e5b..4b2c7306 100644 --- a/pmb2_controller_configuration/CHANGELOG.rst +++ b/pmb2_controller_configuration/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package pmb2_controller_configuration ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix missing dependencies +* Contributors: Victor Lopez + 4.0.0 (2021-07-12) ------------------ * Correct dependency name diff --git a/pmb2_description/CHANGELOG.rst b/pmb2_description/CHANGELOG.rst index c7d5aa7b..41ea0a6b 100644 --- a/pmb2_description/CHANGELOG.rst +++ b/pmb2_description/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package pmb2_description ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix missing dependencies +* Contributors: Victor Lopez + 4.0.0 (2021-07-12) ------------------ * Set back old version number before release diff --git a/pmb2_robot/CHANGELOG.rst b/pmb2_robot/CHANGELOG.rst index a5dc5b84..08ddf242 100644 --- a/pmb2_robot/CHANGELOG.rst +++ b/pmb2_robot/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package pmb2_robot ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.0.0 (2021-07-12) ------------------ * Set back old version number before release From 26de84235c157ce78953a629ac566eabc144c7fc Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Thu, 15 Jul 2021 11:58:02 +0200 Subject: [PATCH 45/57] 4.0.1 --- pmb2_bringup/CHANGELOG.rst | 4 ++-- pmb2_bringup/package.xml | 2 +- pmb2_controller_configuration/CHANGELOG.rst | 4 ++-- pmb2_controller_configuration/package.xml | 2 +- pmb2_description/CHANGELOG.rst | 4 ++-- pmb2_description/package.xml | 2 +- pmb2_robot/CHANGELOG.rst | 4 ++-- pmb2_robot/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/pmb2_bringup/CHANGELOG.rst b/pmb2_bringup/CHANGELOG.rst index 8906e1b0..b7e3ff9c 100644 --- a/pmb2_bringup/CHANGELOG.rst +++ b/pmb2_bringup/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pmb2_bringup ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.1 (2021-07-15) +------------------ * Fix missing dependencies * Contributors: Victor Lopez diff --git a/pmb2_bringup/package.xml b/pmb2_bringup/package.xml index f6daf181..43cf0f83 100644 --- a/pmb2_bringup/package.xml +++ b/pmb2_bringup/package.xml @@ -1,7 +1,7 @@ pmb2_bringup - 4.0.0 + 4.0.1 Launch files and scripts needed to bring up the ROS nodes of a PMB2 robot. Victor Lopez Apache License 2.0 diff --git a/pmb2_controller_configuration/CHANGELOG.rst b/pmb2_controller_configuration/CHANGELOG.rst index 4b2c7306..2464daf4 100644 --- a/pmb2_controller_configuration/CHANGELOG.rst +++ b/pmb2_controller_configuration/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pmb2_controller_configuration ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.1 (2021-07-15) +------------------ * Fix missing dependencies * Contributors: Victor Lopez diff --git a/pmb2_controller_configuration/package.xml b/pmb2_controller_configuration/package.xml index a0234583..dabf49c7 100644 --- a/pmb2_controller_configuration/package.xml +++ b/pmb2_controller_configuration/package.xml @@ -1,7 +1,7 @@ pmb2_controller_configuration - 4.0.0 + 4.0.1 Launch files and scripts needed to configure the controllers of the PMB2 robot. Victor Lopez diff --git a/pmb2_description/CHANGELOG.rst b/pmb2_description/CHANGELOG.rst index 41ea0a6b..3bb3c95e 100644 --- a/pmb2_description/CHANGELOG.rst +++ b/pmb2_description/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pmb2_description ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.1 (2021-07-15) +------------------ * Fix missing dependencies * Contributors: Victor Lopez diff --git a/pmb2_description/package.xml b/pmb2_description/package.xml index a6d4ba91..9f11c5ef 100644 --- a/pmb2_description/package.xml +++ b/pmb2_description/package.xml @@ -1,7 +1,7 @@ pmb2_description - 4.0.0 + 4.0.1 Mechanical, kinematic, visual, etc. description of the PMB2 robot. The files in this package are parsed and used by diff --git a/pmb2_robot/CHANGELOG.rst b/pmb2_robot/CHANGELOG.rst index 08ddf242..a84c212a 100644 --- a/pmb2_robot/CHANGELOG.rst +++ b/pmb2_robot/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pmb2_robot ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.1 (2021-07-15) +------------------ 4.0.0 (2021-07-12) ------------------ diff --git a/pmb2_robot/package.xml b/pmb2_robot/package.xml index c899d288..549be35a 100644 --- a/pmb2_robot/package.xml +++ b/pmb2_robot/package.xml @@ -1,7 +1,7 @@ pmb2_robot - 4.0.0 + 4.0.1 PMB2 robot description and launch files Victor Lopez Apache License 2.0 From febfa9b7c063178c527f03ecc1dc7b8f5c28f9fe Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Thu, 15 Jul 2021 12:04:09 +0200 Subject: [PATCH 46/57] Fix typo --- pmb2_description/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pmb2_description/package.xml b/pmb2_description/package.xml index 9f11c5ef..b8ce7637 100644 --- a/pmb2_description/package.xml +++ b/pmb2_description/package.xml @@ -24,7 +24,7 @@ ament_lint_auto ament_lint_common - launch_testing_ament + launch_testing_ament_cmake urdf_test From e544b76e2fecbfdaf438e9ed7659c40913426baa Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Thu, 15 Jul 2021 12:04:17 +0200 Subject: [PATCH 47/57] Updated Changelog --- pmb2_bringup/CHANGELOG.rst | 3 +++ pmb2_controller_configuration/CHANGELOG.rst | 3 +++ pmb2_description/CHANGELOG.rst | 5 +++++ pmb2_robot/CHANGELOG.rst | 3 +++ 4 files changed, 14 insertions(+) diff --git a/pmb2_bringup/CHANGELOG.rst b/pmb2_bringup/CHANGELOG.rst index b7e3ff9c..045b8140 100644 --- a/pmb2_bringup/CHANGELOG.rst +++ b/pmb2_bringup/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package pmb2_bringup ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.0.1 (2021-07-15) ------------------ * Fix missing dependencies diff --git a/pmb2_controller_configuration/CHANGELOG.rst b/pmb2_controller_configuration/CHANGELOG.rst index 2464daf4..42a1d935 100644 --- a/pmb2_controller_configuration/CHANGELOG.rst +++ b/pmb2_controller_configuration/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package pmb2_controller_configuration ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.0.1 (2021-07-15) ------------------ * Fix missing dependencies diff --git a/pmb2_description/CHANGELOG.rst b/pmb2_description/CHANGELOG.rst index 3bb3c95e..acc0d32f 100644 --- a/pmb2_description/CHANGELOG.rst +++ b/pmb2_description/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package pmb2_description ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix typo +* Contributors: Victor Lopez + 4.0.1 (2021-07-15) ------------------ * Fix missing dependencies diff --git a/pmb2_robot/CHANGELOG.rst b/pmb2_robot/CHANGELOG.rst index a84c212a..dfe93128 100644 --- a/pmb2_robot/CHANGELOG.rst +++ b/pmb2_robot/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package pmb2_robot ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.0.1 (2021-07-15) ------------------ From 72045a74547b25fe48bf5527756102febd4fe3c8 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Thu, 15 Jul 2021 12:04:18 +0200 Subject: [PATCH 48/57] 4.0.2 --- pmb2_bringup/CHANGELOG.rst | 4 ++-- pmb2_bringup/package.xml | 2 +- pmb2_controller_configuration/CHANGELOG.rst | 4 ++-- pmb2_controller_configuration/package.xml | 2 +- pmb2_description/CHANGELOG.rst | 4 ++-- pmb2_description/package.xml | 2 +- pmb2_robot/CHANGELOG.rst | 4 ++-- pmb2_robot/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/pmb2_bringup/CHANGELOG.rst b/pmb2_bringup/CHANGELOG.rst index 045b8140..4489e38a 100644 --- a/pmb2_bringup/CHANGELOG.rst +++ b/pmb2_bringup/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pmb2_bringup ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.2 (2021-07-15) +------------------ 4.0.1 (2021-07-15) ------------------ diff --git a/pmb2_bringup/package.xml b/pmb2_bringup/package.xml index 43cf0f83..14e8debf 100644 --- a/pmb2_bringup/package.xml +++ b/pmb2_bringup/package.xml @@ -1,7 +1,7 @@ pmb2_bringup - 4.0.1 + 4.0.2 Launch files and scripts needed to bring up the ROS nodes of a PMB2 robot. Victor Lopez Apache License 2.0 diff --git a/pmb2_controller_configuration/CHANGELOG.rst b/pmb2_controller_configuration/CHANGELOG.rst index 42a1d935..6f6e9e06 100644 --- a/pmb2_controller_configuration/CHANGELOG.rst +++ b/pmb2_controller_configuration/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pmb2_controller_configuration ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.2 (2021-07-15) +------------------ 4.0.1 (2021-07-15) ------------------ diff --git a/pmb2_controller_configuration/package.xml b/pmb2_controller_configuration/package.xml index dabf49c7..81c17038 100644 --- a/pmb2_controller_configuration/package.xml +++ b/pmb2_controller_configuration/package.xml @@ -1,7 +1,7 @@ pmb2_controller_configuration - 4.0.1 + 4.0.2 Launch files and scripts needed to configure the controllers of the PMB2 robot. Victor Lopez diff --git a/pmb2_description/CHANGELOG.rst b/pmb2_description/CHANGELOG.rst index acc0d32f..60e61fe9 100644 --- a/pmb2_description/CHANGELOG.rst +++ b/pmb2_description/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pmb2_description ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.2 (2021-07-15) +------------------ * Fix typo * Contributors: Victor Lopez diff --git a/pmb2_description/package.xml b/pmb2_description/package.xml index b8ce7637..be644ceb 100644 --- a/pmb2_description/package.xml +++ b/pmb2_description/package.xml @@ -1,7 +1,7 @@ pmb2_description - 4.0.1 + 4.0.2 Mechanical, kinematic, visual, etc. description of the PMB2 robot. The files in this package are parsed and used by diff --git a/pmb2_robot/CHANGELOG.rst b/pmb2_robot/CHANGELOG.rst index dfe93128..8c1ec226 100644 --- a/pmb2_robot/CHANGELOG.rst +++ b/pmb2_robot/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pmb2_robot ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.2 (2021-07-15) +------------------ 4.0.1 (2021-07-15) ------------------ diff --git a/pmb2_robot/package.xml b/pmb2_robot/package.xml index 549be35a..517366b5 100644 --- a/pmb2_robot/package.xml +++ b/pmb2_robot/package.xml @@ -1,7 +1,7 @@ pmb2_robot - 4.0.1 + 4.0.2 PMB2 robot description and launch files Victor Lopez Apache License 2.0 From b534e03ca5353fb05d1df3181d3819e80f7fc3a3 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Wed, 21 Jul 2021 11:47:47 +0200 Subject: [PATCH 49/57] Cleanup --- pmb2_description/CMakeLists.txt | 4 ---- pmb2_description/package.xml | 1 - 2 files changed, 5 deletions(-) diff --git a/pmb2_description/CMakeLists.txt b/pmb2_description/CMakeLists.txt index 36d5f179..6ae9b849 100644 --- a/pmb2_description/CMakeLists.txt +++ b/pmb2_description/CMakeLists.txt @@ -16,12 +16,8 @@ ament_auto_find_build_dependencies() if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - find_package(urdf_test REQUIRED) - ament_lint_auto_find_test_dependencies() - find_package(launch_testing_ament_cmake) - foreach(laser_model no-laser hokuyo sick-551 sick-561 sick-571) foreach(courier_rgbd_sensors False True) add_launch_test( diff --git a/pmb2_description/package.xml b/pmb2_description/package.xml index be644ceb..58c8fd43 100644 --- a/pmb2_description/package.xml +++ b/pmb2_description/package.xml @@ -25,7 +25,6 @@ ament_lint_auto ament_lint_common launch_testing_ament_cmake - urdf_test From c0aac2e5041da5f294c85de066ba707882db0237 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Fri, 25 Jun 2021 17:08:01 +0200 Subject: [PATCH 50/57] use our world odometry plugin --- pmb2_description/gazebo/gazebo.urdf.xacro | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/pmb2_description/gazebo/gazebo.urdf.xacro b/pmb2_description/gazebo/gazebo.urdf.xacro index 5c9454b2..20be91cc 100644 --- a/pmb2_description/gazebo/gazebo.urdf.xacro +++ b/pmb2_description/gazebo/gazebo.urdf.xacro @@ -27,13 +27,13 @@ - - base_footprint - ground_truth_odom - true - 100.0 + + world + base_footprint + ground_truth_odom + 100.0 - + /${nsp} From 9b3356eb173c0d4a1fda513c421ea24b3541508a Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Thu, 23 Sep 2021 07:33:49 +0000 Subject: [PATCH 51/57] Update to new transmission format --- pmb2_description/urdf/wheels/wheel.ros2_control.xacro | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/pmb2_description/urdf/wheels/wheel.ros2_control.xacro b/pmb2_description/urdf/wheels/wheel.ros2_control.xacro index 77da69f8..49ac9308 100644 --- a/pmb2_description/urdf/wheels/wheel.ros2_control.xacro +++ b/pmb2_description/urdf/wheels/wheel.ros2_control.xacro @@ -18,11 +18,11 @@ - transmission_interface/SimpleTransmission - + transmission_interface/SimpleTransmission + 1.0 - + hardware_interface/VelocityJointInterface hardware_interface/JointStateInterface From 9a523122823e2452beba1cd6761e2f616a78db90 Mon Sep 17 00:00:00 2001 From: cescfolch Date: Wed, 16 Jun 2021 10:09:36 +0000 Subject: [PATCH 52/57] add extra joints to joint state --- .../config/joint_state_broadcaster.yaml | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/pmb2_controller_configuration/config/joint_state_broadcaster.yaml b/pmb2_controller_configuration/config/joint_state_broadcaster.yaml index f057e461..68823f70 100644 --- a/pmb2_controller_configuration/config/joint_state_broadcaster.yaml +++ b/pmb2_controller_configuration/config/joint_state_broadcaster.yaml @@ -3,5 +3,4 @@ type: joint_state_broadcaster/JointStateBroadcaster use_sim_time: True publish_rate: 50.0 - - + extra_joints: ['caster_back_left_1_joint', 'caster_back_left_2_joint', 'caster_front_left_1_joint', 'caster_front_left_2_joint', 'caster_back_right_1_joint', 'caster_back_right_2_joint', 'caster_front_right_1_joint', 'caster_front_right_2_joint', 'suspension_left_joint', 'suspension_right_joint'] From f83cf79d0700be44ff3d423465040ecbf862de3c Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Tue, 19 Oct 2021 13:31:44 +0200 Subject: [PATCH 53/57] Updated Changelog --- pmb2_bringup/CHANGELOG.rst | 3 +++ pmb2_controller_configuration/CHANGELOG.rst | 9 +++++++++ pmb2_description/CHANGELOG.rst | 6 ++++++ pmb2_robot/CHANGELOG.rst | 3 +++ 4 files changed, 21 insertions(+) diff --git a/pmb2_bringup/CHANGELOG.rst b/pmb2_bringup/CHANGELOG.rst index 4489e38a..2b108a6d 100644 --- a/pmb2_bringup/CHANGELOG.rst +++ b/pmb2_bringup/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package pmb2_bringup ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.0.2 (2021-07-15) ------------------ diff --git a/pmb2_controller_configuration/CHANGELOG.rst b/pmb2_controller_configuration/CHANGELOG.rst index 6f6e9e06..9e359f0c 100644 --- a/pmb2_controller_configuration/CHANGELOG.rst +++ b/pmb2_controller_configuration/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package pmb2_controller_configuration ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge branch 'add_extra_joints' into 'foxy-devel' + Add extra joints + See merge request robots/pmb2_robot!74 +* add extra joints to joint state +* add extra joints to joint state +* Contributors: cescfolch, victor + 4.0.2 (2021-07-15) ------------------ diff --git a/pmb2_description/CHANGELOG.rst b/pmb2_description/CHANGELOG.rst index 60e61fe9..a00cf53f 100644 --- a/pmb2_description/CHANGELOG.rst +++ b/pmb2_description/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package pmb2_description ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update to new transmission format +* Cleanup +* Contributors: Victor Lopez + 4.0.2 (2021-07-15) ------------------ * Fix typo diff --git a/pmb2_robot/CHANGELOG.rst b/pmb2_robot/CHANGELOG.rst index 8c1ec226..82b464db 100644 --- a/pmb2_robot/CHANGELOG.rst +++ b/pmb2_robot/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package pmb2_robot ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.0.2 (2021-07-15) ------------------ From fb11ed39a84c9f52bbc57b2d8a912daea38ef183 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Tue, 19 Oct 2021 13:31:45 +0200 Subject: [PATCH 54/57] 4.0.3 --- pmb2_bringup/CHANGELOG.rst | 4 ++-- pmb2_bringup/package.xml | 2 +- pmb2_controller_configuration/CHANGELOG.rst | 4 ++-- pmb2_controller_configuration/package.xml | 2 +- pmb2_description/CHANGELOG.rst | 4 ++-- pmb2_description/package.xml | 2 +- pmb2_robot/CHANGELOG.rst | 4 ++-- pmb2_robot/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/pmb2_bringup/CHANGELOG.rst b/pmb2_bringup/CHANGELOG.rst index 2b108a6d..ff0b002c 100644 --- a/pmb2_bringup/CHANGELOG.rst +++ b/pmb2_bringup/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pmb2_bringup ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.3 (2021-10-19) +------------------ 4.0.2 (2021-07-15) ------------------ diff --git a/pmb2_bringup/package.xml b/pmb2_bringup/package.xml index 14e8debf..29ce16ab 100644 --- a/pmb2_bringup/package.xml +++ b/pmb2_bringup/package.xml @@ -1,7 +1,7 @@ pmb2_bringup - 4.0.2 + 4.0.3 Launch files and scripts needed to bring up the ROS nodes of a PMB2 robot. Victor Lopez Apache License 2.0 diff --git a/pmb2_controller_configuration/CHANGELOG.rst b/pmb2_controller_configuration/CHANGELOG.rst index 9e359f0c..3b909c12 100644 --- a/pmb2_controller_configuration/CHANGELOG.rst +++ b/pmb2_controller_configuration/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pmb2_controller_configuration ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.3 (2021-10-19) +------------------ * Merge branch 'add_extra_joints' into 'foxy-devel' Add extra joints See merge request robots/pmb2_robot!74 diff --git a/pmb2_controller_configuration/package.xml b/pmb2_controller_configuration/package.xml index 81c17038..7553f402 100644 --- a/pmb2_controller_configuration/package.xml +++ b/pmb2_controller_configuration/package.xml @@ -1,7 +1,7 @@ pmb2_controller_configuration - 4.0.2 + 4.0.3 Launch files and scripts needed to configure the controllers of the PMB2 robot. Victor Lopez diff --git a/pmb2_description/CHANGELOG.rst b/pmb2_description/CHANGELOG.rst index a00cf53f..72c7bad0 100644 --- a/pmb2_description/CHANGELOG.rst +++ b/pmb2_description/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pmb2_description ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.3 (2021-10-19) +------------------ * Update to new transmission format * Cleanup * Contributors: Victor Lopez diff --git a/pmb2_description/package.xml b/pmb2_description/package.xml index 58c8fd43..0134e9c3 100644 --- a/pmb2_description/package.xml +++ b/pmb2_description/package.xml @@ -1,7 +1,7 @@ pmb2_description - 4.0.2 + 4.0.3 Mechanical, kinematic, visual, etc. description of the PMB2 robot. The files in this package are parsed and used by diff --git a/pmb2_robot/CHANGELOG.rst b/pmb2_robot/CHANGELOG.rst index 82b464db..d8d082f2 100644 --- a/pmb2_robot/CHANGELOG.rst +++ b/pmb2_robot/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pmb2_robot ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.3 (2021-10-19) +------------------ 4.0.2 (2021-07-15) ------------------ diff --git a/pmb2_robot/package.xml b/pmb2_robot/package.xml index 517366b5..6fbfa8df 100644 --- a/pmb2_robot/package.xml +++ b/pmb2_robot/package.xml @@ -1,7 +1,7 @@ pmb2_robot - 4.0.2 + 4.0.3 PMB2 robot description and launch files Victor Lopez Apache License 2.0 From ee6b659a8f6001c378c43b38d39f823429fbf5b5 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Tue, 19 Oct 2021 13:13:22 +0200 Subject: [PATCH 55/57] Add missing exec dependency --- pmb2_description/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/pmb2_description/package.xml b/pmb2_description/package.xml index 0134e9c3..1b58ce9c 100644 --- a/pmb2_description/package.xml +++ b/pmb2_description/package.xml @@ -21,6 +21,7 @@ launch_ros launch_pal joint_state_publisher_gui + pmb2_controller_configuration ament_lint_auto ament_lint_common From 2b84b9a3cb04aa41dc5a4ecc9b378820000c6c1f Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Tue, 19 Oct 2021 13:46:48 +0200 Subject: [PATCH 56/57] Updated Changelog --- pmb2_bringup/CHANGELOG.rst | 3 +++ pmb2_controller_configuration/CHANGELOG.rst | 3 +++ pmb2_description/CHANGELOG.rst | 5 +++++ pmb2_robot/CHANGELOG.rst | 3 +++ 4 files changed, 14 insertions(+) diff --git a/pmb2_bringup/CHANGELOG.rst b/pmb2_bringup/CHANGELOG.rst index ff0b002c..867485ab 100644 --- a/pmb2_bringup/CHANGELOG.rst +++ b/pmb2_bringup/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package pmb2_bringup ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.0.3 (2021-10-19) ------------------ diff --git a/pmb2_controller_configuration/CHANGELOG.rst b/pmb2_controller_configuration/CHANGELOG.rst index 3b909c12..5c6a0019 100644 --- a/pmb2_controller_configuration/CHANGELOG.rst +++ b/pmb2_controller_configuration/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package pmb2_controller_configuration ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.0.3 (2021-10-19) ------------------ * Merge branch 'add_extra_joints' into 'foxy-devel' diff --git a/pmb2_description/CHANGELOG.rst b/pmb2_description/CHANGELOG.rst index 72c7bad0..9c1f5783 100644 --- a/pmb2_description/CHANGELOG.rst +++ b/pmb2_description/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package pmb2_description ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing exec dependency +* Contributors: Victor Lopez + 4.0.3 (2021-10-19) ------------------ * Update to new transmission format diff --git a/pmb2_robot/CHANGELOG.rst b/pmb2_robot/CHANGELOG.rst index d8d082f2..ac259d12 100644 --- a/pmb2_robot/CHANGELOG.rst +++ b/pmb2_robot/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package pmb2_robot ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.0.3 (2021-10-19) ------------------ From 18190aa3237e1e21c4c9eaf39e726dbc53e05830 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Tue, 19 Oct 2021 13:46:49 +0200 Subject: [PATCH 57/57] 4.0.4 --- pmb2_bringup/CHANGELOG.rst | 4 ++-- pmb2_bringup/package.xml | 2 +- pmb2_controller_configuration/CHANGELOG.rst | 4 ++-- pmb2_controller_configuration/package.xml | 2 +- pmb2_description/CHANGELOG.rst | 4 ++-- pmb2_description/package.xml | 2 +- pmb2_robot/CHANGELOG.rst | 4 ++-- pmb2_robot/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/pmb2_bringup/CHANGELOG.rst b/pmb2_bringup/CHANGELOG.rst index 867485ab..35f27a9a 100644 --- a/pmb2_bringup/CHANGELOG.rst +++ b/pmb2_bringup/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pmb2_bringup ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.4 (2021-10-19) +------------------ 4.0.3 (2021-10-19) ------------------ diff --git a/pmb2_bringup/package.xml b/pmb2_bringup/package.xml index 29ce16ab..e5ee887d 100644 --- a/pmb2_bringup/package.xml +++ b/pmb2_bringup/package.xml @@ -1,7 +1,7 @@ pmb2_bringup - 4.0.3 + 4.0.4 Launch files and scripts needed to bring up the ROS nodes of a PMB2 robot. Victor Lopez Apache License 2.0 diff --git a/pmb2_controller_configuration/CHANGELOG.rst b/pmb2_controller_configuration/CHANGELOG.rst index 5c6a0019..2dd1603e 100644 --- a/pmb2_controller_configuration/CHANGELOG.rst +++ b/pmb2_controller_configuration/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pmb2_controller_configuration ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.4 (2021-10-19) +------------------ 4.0.3 (2021-10-19) ------------------ diff --git a/pmb2_controller_configuration/package.xml b/pmb2_controller_configuration/package.xml index 7553f402..4870609d 100644 --- a/pmb2_controller_configuration/package.xml +++ b/pmb2_controller_configuration/package.xml @@ -1,7 +1,7 @@ pmb2_controller_configuration - 4.0.3 + 4.0.4 Launch files and scripts needed to configure the controllers of the PMB2 robot. Victor Lopez diff --git a/pmb2_description/CHANGELOG.rst b/pmb2_description/CHANGELOG.rst index 9c1f5783..b2421edc 100644 --- a/pmb2_description/CHANGELOG.rst +++ b/pmb2_description/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pmb2_description ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.4 (2021-10-19) +------------------ * Add missing exec dependency * Contributors: Victor Lopez diff --git a/pmb2_description/package.xml b/pmb2_description/package.xml index 1b58ce9c..3b8ae41f 100644 --- a/pmb2_description/package.xml +++ b/pmb2_description/package.xml @@ -1,7 +1,7 @@ pmb2_description - 4.0.3 + 4.0.4 Mechanical, kinematic, visual, etc. description of the PMB2 robot. The files in this package are parsed and used by diff --git a/pmb2_robot/CHANGELOG.rst b/pmb2_robot/CHANGELOG.rst index ac259d12..59d27502 100644 --- a/pmb2_robot/CHANGELOG.rst +++ b/pmb2_robot/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pmb2_robot ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.4 (2021-10-19) +------------------ 4.0.3 (2021-10-19) ------------------ diff --git a/pmb2_robot/package.xml b/pmb2_robot/package.xml index 6fbfa8df..94c42178 100644 --- a/pmb2_robot/package.xml +++ b/pmb2_robot/package.xml @@ -1,7 +1,7 @@ pmb2_robot - 4.0.3 + 4.0.4 PMB2 robot description and launch files Victor Lopez Apache License 2.0