ros2/rclcpp

Retrieving(get_parameter()) value of parameter of type std::vector<int> won't build

Pratham-Pandey opened this issue · 3 comments

Bug report

Required Info:

  • Operating System:
    • Ubuntu 22.04 MATE
  • Installation type:
    • binaries
  • Version or commit hash:
    • Iron
  • DDS implementation:
    • Default(have not changed it)
  • Client library (if applicable):
    • rclcpp

Steps to reproduce issue

  1. Create a ROS2 package
ros2 pkg create --build-type ament_cmake vector_int_pkg
  1. Create a simple node the declares a parameter of type std::vector in file src/test_vector_int.cpp
#include "rclcpp/rclcpp.hpp"

int main(int argc, char ** argv)
{
  rclcpp::init(argc, argv);

  std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("vector_integer_parameters");

  // Declaring Params
  node->declare_parameter("param", std::vector<int>{});

  // Getting Params
  std::vector<int> clamps_iters = {1,2,3};
  node->get_parameter("param", clamps_iters);

  rclcpp::spin(node);
  rclcpp::shutdown();
}
  1. Adjust the CMakeLists.txt file
cmake_minimum_required(VERSION 3.5)
project(vector_int_pkg)

if (NOT CMAKE_C_STANDARD)
    set(CMAKE_C_STANDARD 99)
endif ()

if (NOT CMAKE_CXX_STANDARD)
    set(CMAKE_CXX_STANDARD 14)
endif ()

if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
    add_compile_options(-Wall -Wextra -Wpedantic)
endif ()

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)


add_executable(test_vector_int src/test_vector_int.cpp)
ament_target_dependencies(test_vector_int rclcpp)

ament_package()
  1. Adjust the package.xml
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
    <name>vector_int_pkg</name>
    <version>0.0.0</version>
    <description>TODO: Package description</description>
    <maintainer email="john@doe.com">John Doe</maintainer>
    <license>TODO: License declaration</license>

    <buildtool_depend>ament_cmake</buildtool_depend>

    <depend>rclcpp</depend>

    <test_depend>ament_lint_auto</test_depend>
    <test_depend>ament_lint_common</test_depend>

    <export>
        <build_type>ament_cmake</build_type>
    </export>
</package>
  1. Build the package
colcon build --packages-select vector_int_pkg 

Expected behavior

The package should build successfully.

Actual behavior

The build fails with the following error:

/home/user/ros_ws/src/vector_int_pkg/src/test_vector_int.cpp:14:22:   required from here
/opt/ros/iron/include/rclcpp/rclcpp/node_impl.hpp:343:17: error: no matching function for call to ‘std::vector<int>::vector(std::vector<long int, std::allocator<long int> >)’
  343 |     parameter = static_cast<ParameterT>(parameter_variant.get_value<ParameterT>());
      |                 ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

Additional information

The issue seems similar to #1585. The only difference is that issue was related to declaring parameter values and this one is related to retrieving values.
NOTE: The code in #1585 is building successfully on my system.

In ParameterValue, std::vector<int> and std::vector<int64_t> are of the same type.

ParameterValue::ParameterValue(const std::vector<int> & int_array_value)
{
value_.integer_array_value.assign(int_array_value.cbegin(), int_array_value.cend());
value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY;
}
ParameterValue::ParameterValue(const std::vector<int64_t> & int_array_value)
{
value_.integer_array_value = int_array_value;
value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY;
}

Both are stored as std::vector<int64_t>.

The returned value when retrieving is also std::vector<int64_t>

template<typename type>
constexpr
typename std::enable_if<
std::is_convertible<
type, const std::vector<int> &>::value, const std::vector<int64_t> &>::type
get() const
{
return get<ParameterType::PARAMETER_INTEGER_ARRAY>();
}

A simple workaround is std::vector<int64_t> clamps_iters = {1,2,3};

Regarding the fix, I'm considering modifying get() for std::vector as the below

  template<typename type>
  typename std::enable_if<
    std::is_convertible<
      type, const std::vector<int> &>::value, const std::vector<int>>::type
  get() const
  {
    std::vector<int> convert;
    auto array = get<ParameterType::PARAMETER_INTEGER_ARRAY>();
    convert.reserve(array.size());
    for(const auto& val : array) {
      convert.push_back(static_cast<int>(val));
    }
    return convert;
  }

Is there a better way?

In ParameterValue, std::vector and std::vector<int64_t> are of the same type.

  • If both are of same type then what is the need of having 2 different functions?
  • Could you explain the actual cause of the issue?

In ParameterValue, std::vector and std::vector<int64_t> are of the same type.

std::vector -> std:vector<int>

If both are of same type then what is the need of having 2 different functions?
Could you explain the actual cause of the issue?

I think you mean these 2 functions.

template<typename type>
constexpr
typename std::enable_if<
std::is_convertible<
type, const std::vector<int> &>::value, const std::vector<int64_t> &>::type
get() const
{
return get<ParameterType::PARAMETER_INTEGER_ARRAY>();
}
template<typename type>
constexpr
typename std::enable_if<
std::is_convertible<
type, const std::vector<int64_t> &>::value, const std::vector<int64_t> &>::type
get() const
{
return get<ParameterType::PARAMETER_INTEGER_ARRAY>();
}

While executing node->get_parameter("param", clamps_iters);
Regardless of whether the type of clamps_iters is std::vector or std::vector<int64_t>, the function above returns the internally stored std::vector<int64_t>.

The issue lies in the static conversion here. While the type of clamps_iters is std::vector<int>, it wants to convert std::vector<int64_t> to std:vector<int>. But there's no default conversion function.

parameter = static_cast<ParameterT>(parameter_variant.get_value<ParameterT>());