Skip to content
Snippets Groups Projects
Commit 229f8eb6 authored by David Tiede's avatar David Tiede
Browse files

Move to pose on message

parent 88866988
Branches
No related tags found
No related merge requests found
#include "std_msgs/String.h"
#include <moveit/move_group_interface/move_group_interface.h>
int main(int argc, char** argv)
void move_toCallback(const std_msgs::String::ConstPtr& msg)
{
ros::init(argc, argv, "intro_package");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
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);
......@@ -16,24 +14,60 @@ int main(int argc, char** argv)
group.setStartStateToCurrentState();
group.setPlanningTime(5.0);
std::vector<geometry_msgs::Pose> waypoints;
geometry_msgs::Pose start_pose = group.getCurrentPose().pose;
// 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;
target_pose_1.position.z = 0.6;
target_pose_1.position.y = -0.6;
target_pose_1.position.x = 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("FINISHED");
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();
ros::shutdown();
return 0;
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment