IntroPackage.cpp

#include <moveit/move_group_interface/move_group_interface.h>


int main(int argc, char** argv)
{
    ros::init(argc, argv, "intro_package");
    ros::NodeHandle node_handle;
    ros::AsyncSpinner spinner(1);
    spinner.start();

    static const std::string PLANNING_GROUP = "panda_arm";
    moveit::planning_interface::MoveGroupInterface group(PLANNING_GROUP);

    ros::Duration(3.0).sleep();

    group.setStartStateToCurrentState();
    group.setPlanningTime(5.0);

    std::vector<geometry_msgs::Pose> waypoints;

    geometry_msgs::Pose start_pose = group.getCurrentPose().pose;
    geometry_msgs::Pose target_pose_1 = group.getCurrentPose().pose;

    target_pose_1.position.z = 0.6;
    target_pose_1.position.y = -0.6;
    target_pose_1.position.x = 0;
    waypoints.push_back(target_pose_1);

    moveit::planning_interface::MoveGroupInterface::Plan plan;
    moveit_msgs::RobotTrajectory trajectory_msg;
    group.computeCartesianPath(waypoints, 0.01, 0.0, trajectory_msg, true);
    plan.trajectory_ = trajectory_msg;

    group.execute(plan);
    ROS_INFO("FINISHED");

    ros::shutdown();
    return 0;
}