trajectories_publisher.cpp

//
// Created by qiayuan on 2022/7/24.
//

#include "cbf_navigation/trajectories_publisher.h"

#include <ocs2_core/Types.h>
#include <ocs2_core/misc/LoadData.h>
#include <angles/angles.h>

using namespace ocs2;

namespace
{
scalar_t TARGET_DISPLACEMENT_VELOCITY;
scalar_t TARGET_ROTATION_VELOCITY;
scalar_t COM_HEIGHT;
vector_t DEFAULT_JOINT_STATE(12);
}  // namespace

scalar_t estimateTimeToTarget(const vector_t& desired_base_displacement)
{
  const scalar_t& dx = desired_base_displacement(0);
  const scalar_t& dy = desired_base_displacement(1);
  const scalar_t& dyaw = desired_base_displacement(3);
  const scalar_t rotation_time = std::abs(dyaw) / TARGET_ROTATION_VELOCITY;
  const scalar_t displacement = std::sqrt(dx * dx + dy * dy);
  const scalar_t displacement_time = displacement / TARGET_DISPLACEMENT_VELOCITY;
  return std::max(rotation_time, displacement_time);
}

TargetTrajectories pathToTargetTrajectories(const std::vector<geometry_msgs::PoseStamped>& path,
                                            const SystemObservation& observation)
{
  scalar_array_t time_trajectory(path.size());
  vector_array_t state_trajectory(path.size(), vector_t::Zero(observation.state.size()));
  vector_t current_pose = observation.state.segment<6>(6);
  current_pose(2) = COM_HEIGHT;
  current_pose(4) = 0;
  current_pose(5) = 0;
  time_trajectory[0] = observation.time;
  state_trajectory[0] << vector_t::Zero(6), current_pose, DEFAULT_JOINT_STATE;

  for (size_t i = 1; i < path.size(); ++i)
  {
    const vector_t last_pose = state_trajectory[i - 1].segment<6>(6);

    const vector_t desired_pose = [&]() {
      vector_t pose(6);
      pose(0) = path[i].pose.position.x;
      pose(1) = path[i].pose.position.y;
      pose(2) = COM_HEIGHT;
      Eigen::Quaterniond q = Eigen::Quaterniond(path[i].pose.orientation.w, path[i].pose.orientation.x,
                                                path[i].pose.orientation.y, path[i].pose.orientation.z);
      scalar_t siny_cosp = 2 * (q.w() * q.z() + q.x() * q.y());
      scalar_t cosy_cosp = 1 - 2 * (q.y() * q.y() + q.z() * q.z());
      pose(3) = std::atan2(siny_cosp, cosy_cosp);
      pose(3) = last_pose(3) + angles::normalize_angle(pose(3) - last_pose(3));
      pose(4) = 0;
      pose(5) = 0;
      return pose;
    }();
    const vector_t delta_pos = desired_pose - last_pose;
    scalar_t dt = estimateTimeToTarget(delta_pos);
    const vector_t desired_momentum = [&]() {
      vector_t momentum = vector_t::Zero(6);
      momentum(0) = delta_pos(0) / dt;
      momentum(1) = delta_pos(1) / dt;
      return momentum;
    }();
    state_trajectory[i] << desired_momentum, desired_pose, DEFAULT_JOINT_STATE;
    time_trajectory[i] = time_trajectory[i - 1] + dt;
  }
  state_trajectory[path.size() - 1].head(6) = vector_t::Zero(6);

  const vector_array_t input_trajectory(path.size(), vector_t::Zero(observation.input.size()));
  return { time_trajectory, state_trajectory, input_trajectory };
}

int main(int argc, char* argv[])
{
  const std::string robot_name = "legged_robot";

  // Initialize ros node
  ::ros::init(argc, argv, robot_name + "_target");
  ::ros::NodeHandle node_handle;
  // Get node parameters
  std::string reference_file, task_file;
  node_handle.getParam("/reference_file", reference_file);
  node_handle.getParam("/task_file", task_file);

  loadData::loadCppDataType(reference_file, "comHeight", COM_HEIGHT);
  loadData::loadEigenMatrix(reference_file, "defaultJointState", DEFAULT_JOINT_STATE);
  loadData::loadCppDataType(reference_file, "targetRotationVelocity", TARGET_ROTATION_VELOCITY);
  loadData::loadCppDataType(reference_file, "targetDisplacementVelocity", TARGET_DISPLACEMENT_VELOCITY);

  cbf::TrajectoriesPublisher target_pose_command(node_handle, robot_name, &pathToTargetTrajectories);
  ros::Rate loop_rate(5);
  while (ros::ok())
  {
    target_pose_command.plan();
    ros::spinOnce();
    loop_rate.sleep();
  }
  // Successful exit
  return 0;
}

trajectories_publisher.h

//
// Created by qiayuan on 2022/7/24.
//

#pragma once

#include <utility>
#include <ros/subscriber.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/GetPlan.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

#include <ocs2_mpc/SystemObservation.h>
#include <ocs2_robotic_tools/common/RotationTransforms.h>
#include <ocs2_ros_interfaces/command/TargetTrajectoriesRosPublisher.h>

namespace cbf
{
using namespace ocs2;
using namespace Eigen;

class TrajectoriesPublisher final
{
  using PathToTargetTrajectories = std::function<TargetTrajectories(const std::vector<geometry_msgs::PoseStamped>&,
                                                                    const SystemObservation& observation)>;

public:
  TrajectoriesPublisher(::ros::NodeHandle& nh, const std::string& topic_prefix,
                        PathToTargetTrajectories plan_to_target_trajectories)
    : plan_to_target_trajectories_(std::move(plan_to_target_trajectories)), tf2_(buffer_)
  {
    publisher_.reset(new TargetTrajectoriesRosPublisher(nh, topic_prefix));
    plan_srv_ = nh.serviceClient<nav_msgs::GetPlan>("/global_planner/planner/make_plan");
    plan_srv_.waitForExistence();

    auto observation_callback = [this](const ocs2_msgs::mpc_observation::ConstPtr& msg) {
      observation_ = ros_msg_conversions::readObservationMsg(*msg);
    };
    observation_sub_ =
        nh.subscribe<ocs2_msgs::mpc_observation>(topic_prefix + "_mpc_observation", 1, observation_callback);

    auto goal_callback = [this](const geometry_msgs::PoseStamped::ConstPtr& msg) {
      pose_goal_ = *msg;
      try
      {
        buffer_.transform(pose_goal_, pose_goal_, "map", ros::Duration(0.5));
      }
      catch (tf2::TransformException& ex)
      {
        ROS_WARN("Failure %s\n", ex.what());
      }
      plan();
    };
    goal_sub_ = nh.subscribe<geometry_msgs::PoseStamped>("/move_base_simple/goal", 1, goal_callback);
  }

  void plan()
  {
    if (observation_.time == 0.0 || pose_goal_.header.stamp.isZero())
      return;

    const vector_t start_pose = observation_.state.segment<6>(6);
    Quaterniond start_quat = getQuaternionFromEulerAnglesZyx(Eigen::Vector3d(start_pose.tail(3)));

    nav_msgs::GetPlan srv;
    srv.request.start.header.frame_id = "odom";
    srv.request.start.header.stamp = ros::Time(0);
    srv.request.start.pose.position.x = start_pose(0);
    srv.request.start.pose.position.y = start_pose(1);
    srv.request.start.pose.orientation.x = start_quat.x();
    srv.request.start.pose.orientation.y = start_quat.y();
    srv.request.start.pose.orientation.z = start_quat.z();
    srv.request.start.pose.orientation.w = start_quat.w();
    srv.request.goal = pose_goal_;
    srv.request.goal.header.stamp = ros::Time(0);

    try
    {
      geometry_msgs::TransformStamped map2odom = buffer_.lookupTransform("odom", "map", ros::Time(0)),
                                      odom2map = buffer_.lookupTransform("map", "odom", ros::Time(0));

      tf2::doTransform(srv.request.start, srv.request.start, odom2map);

      if (std::sqrt(std::pow(srv.request.start.pose.position.x - srv.request.goal.pose.position.x, 2) +
                    std::pow(srv.request.start.pose.position.y - srv.request.goal.pose.position.y, 2)) < 0.1)
        return;

      if (plan_srv_.call(srv) && srv.response.plan.poses.size() > 1)
      {
        auto poses = srv.response.plan.poses;
        for (auto& pose : poses)
          tf2::doTransform(pose, pose, map2odom);
        publisher_->publishTargetTrajectories(plan_to_target_trajectories_(poses, observation_));
      }
    }
    catch (tf2::TransformException& ex)
    {
      ROS_WARN("Failure %s\n", ex.what());
      return;
    }
  }

private:
  PathToTargetTrajectories plan_to_target_trajectories_;

  std::unique_ptr<TargetTrajectoriesRosPublisher> publisher_;
  ros::ServiceClient plan_srv_;
  ros::Subscriber observation_sub_, goal_sub_;
  tf2_ros::Buffer buffer_;
  tf2_ros::TransformListener tf2_;

  SystemObservation observation_;
  geometry_msgs::PoseStamped pose_goal_;
};

}  // namespace cbf