From 229f8eb66cac6b10c6f41cc82bf8e0d5bf300fba Mon Sep 17 00:00:00 2001
From: David Tiede <david.tiede@mailbox.tu-dresden.de>
Date: Tue, 17 Nov 2020 15:36:32 +0100
Subject: [PATCH] Move to pose on message
---
src/IntroPackage.cpp | 64 +++++++++++++++++++++++++++++++++-----------
1 file changed, 49 insertions(+), 15 deletions(-)
diff --git a/src/IntroPackage.cpp b/src/IntroPackage.cpp
index 29a21ae..e88544d 100644
--- a/src/IntroPackage.cpp
+++ b/src/IntroPackage.cpp
@@ -1,12 +1,10 @@
+#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;
- waypoints.push_back(target_pose_1);
+ 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;
}
--
GitLab