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

[Kotochleb]

Feature request

Feature description

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

Following snippet reproduces this case.

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>());

Implementation considerations

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;
}

To my knowledge, this should be the way to baypass that issue. At least, the way I understood Eigen documentation.