Select Git revision
IntroPackage.cpp
-
David Tiede authoredDavid Tiede authored
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;
}