ros_utils.h

#ifndef SPIRIT_ROS_UTILS_H
#define SPIRIT_ROS_UTILS_H
 
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Vector3.h>
#include <quad_utils/math_utils.h>
#include <ros/ros.h>
#include <std_msgs/Header.h>
 
namespace quad_utils {
inline double getROSMessageAgeInMs(std_msgs::Header header,
                                   ros::Time t_compare) {
  return (t_compare - header.stamp).toSec() * 1000.0;
}
 
inline double getROSMessageAgeInMs(std_msgs::Header header) {
  ros::Time t_compare = ros::Time::now();
  return quad_utils::getROSMessageAgeInMs(header, t_compare);
}
 
inline double getDurationSinceTime(ros::Time plan_start) {
  return (ros::Time::now() - plan_start).toSec();
}
 
inline void getPlanIndex(ros::Time plan_start, double dt, int &index,
                         double &first_element_duration) {
  double duration = getDurationSinceTime(plan_start);
  index = std::floor(duration / dt);
  first_element_duration = (index + 1) * dt - duration;
}
 
template <class ParamType>
inline bool loadROSParam(ros::NodeHandle nh, std::string paramName,
                         ParamType &varName) {
  if (!nh.getParam(paramName, varName)) {
    ROS_ERROR("Can't find param %s from parameter server", paramName.c_str());
    return false;
  }
  return true;
}
 
template <class ParamType>
inline bool loadROSParamDefault(ros::NodeHandle nh, std::string paramName,
                                ParamType &varName, ParamType defaultVal) {
  if (!nh.getParam(paramName, varName)) {
    varName = defaultVal;
    ROS_INFO("Can't find param %s on rosparam server, loading default value.",
             paramName.c_str());
    return false;
  }
  return true;
}
 
// /**
//  * @brief Interpolate two headers
//  * @param[out] msg State message to popluate
//  * @param[in] stamp Timestamp for the state message
//  * @param[in] frame Frame_id for the state message
//  */
// void updateStateHeaders(quad_msgs::RobotState &msg, ros::Time stamp,
// std::string frame);
 
void updateStateHeaders(quad_msgs::RobotState &msg, ros::Time stamp,
                        std::string frame, int traj_index);
 
void interpHeader(std_msgs::Header header_1, std_msgs::Header header_2,
                  double t_interp, std_msgs::Header &interp_header);
 
void interpOdometry(quad_msgs::BodyState state_1, quad_msgs::BodyState state_2,
                    double t_interp, quad_msgs::BodyState &interp_state);
 
void interpJointState(sensor_msgs::JointState state_1,
                      sensor_msgs::JointState state_2, double t_interp,
                      sensor_msgs::JointState &interp_state);
 
void interpMultiFootState(quad_msgs::MultiFootState state_1,
                          quad_msgs::MultiFootState state_2, double t_interp,
                          quad_msgs::MultiFootState &interp_state);
 
void interpGRFArray(quad_msgs::GRFArray state_1, quad_msgs::GRFArray state_2,
                    double t_interp, quad_msgs::GRFArray &interp_state);
 
void interpRobotState(quad_msgs::RobotState state_1,
                      quad_msgs::RobotState state_2, double t_interp,
                      quad_msgs::RobotState &interp_state);
 
void interpRobotPlan(quad_msgs::RobotPlan msg, double t,
                     quad_msgs::RobotState &interp_state,
                     int &interp_primitive_id, quad_msgs::GRFArray &interp_grf);
 
quad_msgs::MultiFootState interpMultiFootPlanContinuous(
    quad_msgs::MultiFootPlanContinuous msg, double t);
 
// /**
//  * @brief Interpolate data from a robot state trajectory message.
//  * @param[in] msg robot state trajectory message
//  * @param[in] t Time since beginning of trajectory (will return last state if
//  * too large)
//  * @return Robot state message
//  */
// quad_msgs::RobotState interpRobotStateTraj(quad_msgs::RobotStateTrajectory
// msg,
//                                            double t);
 
void ikRobotState(const quad_utils::QuadKD &kinematics,
                  quad_msgs::BodyState body_state,
                  quad_msgs::MultiFootState multi_foot_state,
                  sensor_msgs::JointState &joint_state);
 
void ikRobotState(const quad_utils::QuadKD &kinematics,
                  quad_msgs::RobotState &state);
 
void fkRobotState(const quad_utils::QuadKD &kinematics,
                  quad_msgs::BodyState body_state,
                  sensor_msgs::JointState joint_state,
                  quad_msgs::MultiFootState &multi_foot_state);
 
void fkRobotState(const quad_utils::QuadKD &kinematics,
                  quad_msgs::RobotState &state);
 
quad_msgs::BodyState eigenToBodyStateMsg(const Eigen::VectorXd &state);
 
Eigen::VectorXd bodyStateMsgToEigen(const quad_msgs::BodyState &body);
 
void eigenToGRFArrayMsg(Eigen::VectorXd grf_array,
                        quad_msgs::MultiFootState multi_foot_state_msg,
                        quad_msgs::GRFArray &grf_msg);
 
Eigen::VectorXd grfArrayMsgToEigen(const quad_msgs::GRFArray &grf_array_msg_);
 
void footStateMsgToEigen(const quad_msgs::FootState &foot_state_msg,
                         Eigen::Vector3d &foot_position);
 
void multiFootStateMsgToEigen(
    const quad_msgs::MultiFootState &multi_foot_state_msg,
    Eigen::VectorXd &foot_positions);
 
void multiFootStateMsgToEigen(
    const quad_msgs::MultiFootState &multi_foot_state_msg,
    Eigen::VectorXd &foot_positions, Eigen::VectorXd &foot_velocities);
 
void multiFootStateMsgToEigen(
    const quad_msgs::MultiFootState &multi_foot_state_msg,
    Eigen::VectorXd &foot_positions, Eigen::VectorXd &foot_velocities,
    Eigen::VectorXd &foot_acceleration);
 
void eigenToFootStateMsg(Eigen::VectorXd foot_position,
                         Eigen::VectorXd foot_velocity,
                         quad_msgs::FootState &foot_state_msg);
 
void eigenToFootStateMsg(Eigen::VectorXd foot_position,
                         Eigen::VectorXd foot_velocity,
                         Eigen::VectorXd foot_acceleration,
                         quad_msgs::FootState &foot_state_msg);
 
void eigenToVector(const Eigen::VectorXd &eigen_vec, std::vector<double> &vec);
 
void vectorToEigen(const std::vector<double> &vec, Eigen::VectorXd &eigen_vec);
 
void Eigen3ToVector3Msg(const Eigen::Vector3d &eigen_vec,
                        geometry_msgs::Vector3 &vec);
 
void vector3MsgToEigen(const geometry_msgs::Vector3 &vec,
                       Eigen::Vector3d &eigen_vec);
 
void Eigen3ToPointMsg(const Eigen::Vector3d &eigen_vec,
                      geometry_msgs::Point &vec);
 
void pointMsgToEigen(const geometry_msgs::Point &vec,
                     Eigen::Vector3d &eigen_vec);
}  // namespace quad_utils
 
#endif