Ask Your Question
0

InteractiveMarker sort of inertia?

asked 2015-10-16 11:52:33 -0500

alextoind gravatar image

updated 2015-10-21 15:32:52 -0500

Is there an easy way of implement a sort of inertia for visualization_msgs::InteractiveMarker?

I'm dealing with a MOVE_ROTATE InteractiveMarker in rviz and it is simple to move it too fast causing unexpected behaviour in my application. I think I could handle it with a saturation in the marker callback and a subsequent update of the pose of the marker with the satured value, but is there a neater way of accomplishing such a behaviour?

Thank you all.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2015-10-21 15:32:32 -0500

alextoind gravatar image

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 &current) {
  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.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2015-10-16 11:52:33 -0500

Seen: 112 times

Last updated: Oct 21 '15