ros / geometry2

A set of ROS packages for keeping track of coordinate transforms.
191 stars 279 forks source link

[tf_eigen] Usage of MatrixBase in functions to allow block operations #572

Closed Kotochleb closed 1 month ago

Kotochleb commented 1 month ago

When trying to pass blocks of vectors to functions converting from message to Eigen I got an error:

error: cannot bind non-const lvalue reference of type

To reproduce that, you can try the following snippet.

geometry_msgs::msg::Wrench wrench_msg;
Eigen::Matrix<double, 6, 1> wrench_vect;

tf2::fromMsg(wrench_msg.force, wrench_vect.template head<3>());
tf2::fromMsg(wrench_msg.torque, wrench_vect.template tail<3>());

I am wondering if using templates to allow such operations makes any sense. Especially, that for function converting from geometry_msgs/Vector3 to Eigen::Vector3d implementing it, changes it from a simple function like this:

inline
void fromMsg(const geometry_msgs::msg::Vector3 & msg, Eigen::Vector3d & out)
{
  out.x() = msg.x;
  out.y() = msg.y;
  out.z() = msg.z;
}

to that monstrosity:

template <typename Derived> inline
void fromMsg(const geometry_msgs::msg::Vector3 & msg, ::Eigen::MatrixBase<Derived> const & out)
{
  assert(out.size() == 3 && "Passed Vector is not size of 3.");
  const_cast< ::Eigen::MatrixBase<Derived>& >(out)(0) = msg.x;
  const_cast< ::Eigen::MatrixBase<Derived>& >(out)(1) = msg.y;
  const_cast< ::Eigen::MatrixBase<Derived>& >(out)(2) = msg.z;
}
Kotochleb commented 1 month ago

I am closing. My mistake. I planned to open it on ros2/geometry2 not here