Skip to content
Snippets Groups Projects
Select Git revision
  • f1d7dea4a60b9eebc3131c1297895d4cff32f4dd
  • master default protected
2 results

pose_replayer.cpp

Blame
    • pose_replayer.cpp 2.95 KiB 
    //
// 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>

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();

    node_handle.getParam("x_transform", x_transform);
    node_handle.getParam("y_transform", y_transform);
    node_handle.getParam("z_transform", z_transform);

    node_handle.getParam("x_rotation", x_rotation);
    node_handle.getParam("y_rotation", y_rotation);
    node_handle.getParam("z_rotation", z_rotation);
    node_handle.getParam("w_rotation", w_rotation);

    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;
}