Skip to content
Snippets Groups Projects
Select Git revision
  • master default protected
1 result

IntroPackage.cpp

Blame
  • IntroPackage.cpp 2.11 KiB
    #include "std_msgs/String.h"
    #include <moveit/move_group_interface/move_group_interface.h>
    
    
    void move_toCallback(const std_msgs::String::ConstPtr& msg)
    {
        ROS_INFO("INTRO PACKAGE: Recevied message: '%s'", msg->data.c_str());
    
        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);
    
        // pose A
        geometry_msgs::Pose target_pose_1 = group.getCurrentPose().pose;
        target_pose_1.position.z = 0.4;
        target_pose_1.position.y = 0.4;
        target_pose_1.position.x = 0.8;
        target_pose_1.orientation.x = 0;
        target_pose_1.orientation.y = 0;
        target_pose_1.orientation.z = 0;
        target_pose_1.orientation.w = 1;
    
        // pose B
        geometry_msgs::Pose target_pose_2 = group.getCurrentPose().pose;
        target_pose_2.position.z = 0.4;
        target_pose_2.position.y = 0.4;
        target_pose_2.position.x = 0.8;
        target_pose_2.orientation.x = 0;
        target_pose_2.orientation.y = 0;
        target_pose_2.orientation.z = 0;
        target_pose_2.orientation.w = 0;
    
        std::vector<geometry_msgs::Pose> waypoints;
        std::string strA ("A");
        std::string strB ("B");
        if (strA == msg->data) {
            waypoints.push_back(target_pose_1);
        } else if (strB == msg->data) {
            waypoints.push_back(target_pose_2);
        } else {
            ROS_WARN("INTRO PACKAGE: Wrong message.");
            return;
        }
    
        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("INTRO PACKAGE: Finished movement.");
    
    }
    
    
    int main(int argc, char** argv)
    {
        ros::init(argc, argv, "intro_package");
        ros::NodeHandle node_handle;
        ros::AsyncSpinner spinner(1);
    
        spinner.start();
    
        ros::Subscriber sub = node_handle.subscribe("move_to", 1000, move_toCallback);
        ROS_INFO("INTRO PACKAGE: Listening to 'move_to'.");
    
        ros::waitForShutdown();
    
        return 0;
    }