TimedPlanner.cpp

//
// Created by sebastian on 31.03.20.
//
#include <moveit/move_group_interface/move_group_interface.h>

#include <moveit_msgs/DisplayTrajectory.h>
#include <moveit_msgs/CollisionObject.h>

#include <moveit_visual_tools/moveit_visual_tools.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
#include <trajectory_msgs/JointTrajectoryPoint.h>

#include "util/TrajectoryUtil.h"

std::vector<geometry_msgs::Pose> raw_trajectory;
TrajectoryUtil traj_util;

const double default_velocity = 1.0;
const std::string default_planning_mode = TrajectoryUtil::CARTESIAN_PATH;

bool isInitialized = false;
bool isLooping = true;

void
initRawTrajectory(ros::NodeHandle node_handle, moveit::planning_interface::MoveGroupInterface *group, bool useDefault) {

    geometry_msgs::Pose current_pose = group->getCurrentPose().pose;

    if (!traj_util.initWaypoints(raw_trajectory, node_handle, current_pose) && useDefault) {
        // choose a default trajectory
        geometry_msgs::Pose target_pose_1 = group->getCurrentPose().pose;
        geometry_msgs::Pose target_pose_2 = group->getCurrentPose().pose;
        geometry_msgs::Pose target_pose_3 = group->getCurrentPose().pose;
        geometry_msgs::Pose target_pose_4 = group->getCurrentPose().pose;

        target_pose_2.position.z = 0.6;//0.583;
        target_pose_2.position.y = -0.6;//63;
        target_pose_2.position.x = 0;//-0.007;
        raw_trajectory.push_back(target_pose_2);

        target_pose_3.position.z = 0.6;//0.691;
        target_pose_3.position.y = -0.032;
        target_pose_3.position.x = -0.607;
        raw_trajectory.push_back(target_pose_3);

        target_pose_4.position.z = 0.6;
        target_pose_4.position.y = 0.6;//0.509;
        target_pose_4.position.x = 0;//0.039;
        raw_trajectory.push_back(target_pose_4);

        target_pose_1.position.z = group->getCurrentPose().pose.position.z;
        target_pose_1.position.y = group->getCurrentPose().pose.position.y;
        target_pose_1.position.x = group->getCurrentPose().pose.position.x;
        raw_trajectory.push_back(target_pose_1);
    }
}

bool updateRawTrajectory(ros::NodeHandle node_handle, moveit::planning_interface::MoveGroupInterface *group) {

    geometry_msgs::Pose current_pose = group->getCurrentPose().pose;
    raw_trajectory.clear();
    node_handle.setParam("new_trajectory_available", false);

    return traj_util.initWaypoints(raw_trajectory, node_handle, current_pose);
}

void moveRobotToInitialState(ros::NodeHandle node_handle) {

    ROS_INFO("Moving to initial pose of trajectory.");
    moveit::planning_interface::MoveGroupInterface group("panda_arm");

    ros::Publisher display_publisher = node_handle.
            advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
    group.setStartStateToCurrentState();

    std::vector<geometry_msgs::Pose> trajectory_to_init_pose;
    geometry_msgs::Pose target_pose_1 = group.getCurrentPose().pose;

    target_pose_1.position.z = group.getCurrentPose().pose.position.z - 0.4;
    target_pose_1.position.y = group.getCurrentPose().pose.position.y;
    target_pose_1.position.x = group.getCurrentPose().pose.position.x + 0.5;
    trajectory_to_init_pose.push_back(target_pose_1);
    group.setPlanningTime(10.0);

    moveit_msgs::RobotTrajectory trajectory;
    double fraction = group.computeCartesianPath(trajectory_to_init_pose, 0.01, 0.0, trajectory);

    moveit::planning_interface::MoveGroupInterface::Plan cartesian_plan;
    cartesian_plan.trajectory_ = trajectory;
    group.execute(cartesian_plan);
}

void doMotion(const ros::NodeHandle &node_handle, moveit::planning_interface::MoveGroupInterface &group) {

    for (int i = 0; i < raw_trajectory.size(); i++) {

        bool newTrajectoryAvaible = false;
        node_handle.getParam("new_trajectory_available", newTrajectoryAvaible);
        node_handle.getParam("loop_trajectory", isLooping);

        if (newTrajectoryAvaible) {
            if (!updateRawTrajectory(node_handle, &group)) {
                break;
            }
            i = 0;
        }

        // wake up if we have trajectory
        while (raw_trajectory.size() == 0) {
            if (newTrajectoryAvaible) {
                updateRawTrajectory(node_handle, &group);
                i = 0;
                break;
            }
        }

        double velocity = 0.0;
        std::string planning_mode = "";

        if (!node_handle.getParam("robot_speed_factor", velocity)) {
            velocity = default_velocity;
        }

        if (!node_handle.getParam("robot_planning_mode", planning_mode)) {
            planning_mode = default_planning_mode;
        }

        std::cout << "<<<<<<<<<<<<<<<<<<< MODE: " << planning_mode << std::endl;
        moveit::planning_interface::MoveGroupInterface::Plan plan;
        traj_util.computePathToPose(group, plan, raw_trajectory.at(i), planning_mode, velocity);
        group.execute(plan);

        // make sure the robot moves in an infinite circle
        if (i == (raw_trajectory.size() - 1)) {
            if (isLooping) {
                i = -1;
            } else {
                return;
            }
        }
    }
}

int main(int argc, char **argv) {

    // setup this ros-node
    ros::init(argc, argv, "timed_cartesian_planner");
    ros::NodeHandle node_handle;
    ros::AsyncSpinner spinner(1);
    spinner.start();

    // wait for robot init of robot_state_initializer
    ros::Duration(5.0).sleep();
    ROS_INFO(">>>>>>>>>>>>>>>>> START UP FINISHED <<<<<<<<<<<<<<<< ");
    node_handle.setParam("tud_planner_ready", true);

    // Visualization Setup.
    namespace rvt = rviz_visual_tools;
    moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
    visual_tools.deleteAllMarkers();
    visual_tools.loadRemoteControl();

    Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
    text_pose.translation().z() = 1.75;
    visual_tools.publishText(text_pose, "cartesian planner node", rvt::WHITE, rvt::XLARGE);
    visual_tools.trigger();

    // Initialize start state of robot and target trajectory.
    moveit::planning_interface::MoveGroupInterface group("panda_arm");
    moveRobotToInitialState(node_handle);
    initRawTrajectory(node_handle, &group, true);
    node_handle.getParam("loop_trajectory", isLooping);

    // execute the trajectory which consists of single waypoints to allow distinct planning
    doMotion(node_handle, group);

    ros::shutdown();
    return 0;
}