I've made this rough workaround which does not represent the property of inertia, but works in a similar way:
class Class {
public:
...
private:
interactive_markers::InteractiveMarkerServer *interactive_marker_server_;
geometry_msgs::Pose marker_pose_; // initialized in the constructor
...
}
void Class::interactiveMarkerCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) {
// avoids too fast changes in the interactive marker pose (especially for its orientation)
interactiveMarkerGuidance(feedback->pose, marker_pose_); // marker_pose_ is updated every callback
interactive_marker_server_->setPose(feedback->marker_name, marker_pose_, feedback->header);
interactive_marker_server_->applyChanges();
}
void Class::interactiveMarkerGuidance(const geometry_msgs::Pose &target, geometry_msgs::Pose ¤t) {
tf::Pose current_pose, target_pose;
tf::poseMsgToTF(current, current_pose);
tf::poseMsgToTF(target, target_pose);
// distance and theta represent the arrow from current to target pose (the one given by the mouse position)
double distance = tf::tfDistance(current_pose.getOrigin(), target_pose.getOrigin());
double distance_sat = saturation(distance, marker_dist_min_, marker_dist_max_);
double theta = std::atan2(target.position.y - current.position.y, target.position.x - current.position.x);
// angle represents the rotation of the MOVE_ROTATE interactive marker
double current_roll, current_pitch, current_yaw, target_roll, target_pitch, target_yaw;
tf::Matrix3x3(current_pose.getRotation()).getRPY(current_roll, current_pitch, current_yaw);
tf::Matrix3x3(target_pose.getRotation()).getRPY(target_roll, target_pitch, target_yaw);
double angle = angles::shortest_angular_distance(current_yaw, target_yaw);
double angle_sat = saturation(angle, marker_steer_min_, marker_steer_max_);
tf::Transform transform; // the transform with no saturated values would produce the default marker behaviour
transform.setOrigin(tf::Vector3(distance_sat*std::cos(theta - current_yaw), distance_sat*std::sin(theta - current_yaw), 0));
transform.setRotation(tf::createQuaternionFromRPY(0, 0, angle_sat));
current_pose *= transform;
tf::poseTFToMsg(current_pose, current);
}
double Class::saturation(const double &value, const double &min, const double &max) {
return std::min(std::max(value, min), max);
}
I have chosen not to add the interactive marker initialization to keep this answer as neat as possible. Anyway it is very similar to the one in the Interactive Markers Tutorial.
Any further comments or suggestions are welcome.