pose_replayer.cpp

//
// Created by sebastian on 04.05.20.
//
#include <ros/ros.h>

#include <std_msgs/String.h>

#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/CollisionObject.h>
#include <moveit_visual_tools/moveit_visual_tools.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>

#include "panda_teaching/GetCollectedPoses.h"

void replayCallback(const std_msgs::String::ConstPtr &msg, ros::NodeHandle &node_handle) {

    static const std::string PLANNING_GROUP = "panda_arm";
    moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
    const robot_state::JointModelGroup *joint_model_group =
            move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);

    std::vector<double> x_transform, y_transform, z_transform, x_rotation, y_rotation, z_rotation, w_rotation;

    // 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, "Sample constraint planner node", rvt::WHITE, rvt::XLARGE);
    visual_tools.trigger();

    ros::ServiceClient client = node_handle.serviceClient<panda_teaching::GetCollectedPoses>("GetCollectedPoses");
    panda_teaching::GetCollectedPoses srv;

    if (client.call(srv)) {
        for (int i = 0; i < srv.response.x_transform.size(); i++) {
            x_transform.push_back((double) srv.response.x_transform[i]);
            y_transform.push_back((double) srv.response.y_transform[i]);
            z_transform.push_back((double) srv.response.z_transform[i]);

            x_rotation.push_back((double) srv.response.x_rotation[i]);
            y_rotation.push_back((double) srv.response.y_rotation[i]);
            z_rotation.push_back((double) srv.response.z_rotation[i]);
            w_rotation.push_back((double) srv.response.w_rotation[i]);
        }
    } else {
        ROS_ERROR("Failed to fetch saved poses");
        return;
    }

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

        move_group.setStartState(*move_group.getCurrentState());
        geometry_msgs::Pose another_pose;
        another_pose.orientation.x = x_rotation.at(i);
        another_pose.orientation.y = y_rotation.at(i);
        another_pose.orientation.z = z_rotation.at(i);
        another_pose.orientation.w = w_rotation.at(i);
        another_pose.position.x = x_transform.at(i);
        another_pose.position.y = y_transform.at(i);
        another_pose.position.z = z_transform.at(i);
        move_group.setPoseTarget(another_pose);

        moveit::planning_interface::MoveGroupInterface::Plan my_plan;
        bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
        ROS_INFO_NAMED("pose_replayer", "Visualizing plan %s", success ? "" : "FAILED");
        move_group.move();
    }
}

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

    ros::init(argc, argv, "pose_replayer");
    ros::NodeHandle node_handle;
    ros::AsyncSpinner spinner(2);
    spinner.start();

    ros::Subscriber sub = node_handle.subscribe<std_msgs::String>("replayPose", 1000, boost::bind(&replayCallback, _1,
                                                                                                  boost::ref(
                                                                                                          node_handle)));
    ros::waitForShutdown();
    return 0;
}