Method generateGravity

src/debug_visualiser.cpp:539–572

/ * Publishes visualisation markers which represent the estimate of the gravitational acceleration vector. * @param[in] gravity_estimate An estimate of the gravitational acceleration vector ***********************************************************************************************************************/

  * @param[in] gravity_estimate An estimate of the gravitational acceleration vector
***********************************************************************************************************************/
void DebugVisualiser::generateGravity(const Vector3d& gravity_estimate)
{
  visualization_msgs::Marker gravity;
  gravity.header.frame_id = "/base_link";
  gravity.header.stamp = ros::Time::now();
  gravity.ns = "gravity_marker";
  gravity.id = 0;
  gravity.type = visualization_msgs::Marker::ARROW;
  gravity.action = visualization_msgs::Marker::ADD;
  
  Vector3d robot_position = Vector3d::Zero();
  geometry_msgs::Point origin;
  origin.x = robot_position[0];
  origin.y = robot_position[1];
  origin.z = robot_position[2];
  
  Vector3d direction_vector = gravity_estimate.normalized() * 0.1 * sqrt(marker_scale_); //Arrow Length  
  geometry_msgs::Point target;
  target = origin;
  target.x += direction_vector[0];
  target.y += direction_vector[1];
  target.z += direction_vector[2];
  gravity.points.push_back(origin);
  gravity.points.push_back(target);
  gravity.scale.x = 0.01 * sqrt(marker_scale_);
  gravity.scale.y = 0.015 * sqrt(marker_scale_);
  gravity.scale.z = 0.02 * sqrt(marker_scale_);
  gravity.color.r = 1; // YELLOW
  gravity.color.g = 1;
  gravity.color.a = 1;
  gravity.pose = Pose::Identity().convertToPoseMessage();

  gravity_publisher_.publish(gravity);
}

/***********************************************************************************************************************
***********************************************************************************************************************/