diff --git a/adapter_package/src/adapter_package.cpp b/adapter_package/src/adapter_package.cpp index 8a5189da63c1d95fa42ee20952ba3339a99b711b..4b37a9d419a6230e4eb06ef325fef51469a61559 100644 --- a/adapter_package/src/adapter_package.cpp +++ b/adapter_package/src/adapter_package.cpp @@ -26,12 +26,12 @@ void move_toCallback(const std_msgs::String::ConstPtr& msg) // 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.position.z = 0; + target_pose_2.position.y = 0.3; + target_pose_2.position.x = 0.5; + target_pose_2.orientation.x = 0.4; + target_pose_2.orientation.y = 0.7; + target_pose_2.orientation.z = 0.4; target_pose_2.orientation.w = 1; std::vector<geometry_msgs::Pose> waypoints; @@ -46,11 +46,13 @@ void move_toCallback(const std_msgs::String::ConstPtr& msg) 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); + ///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); + group.setPoseTarget(target_pose_2); + group.move(); ROS_INFO("ADAPTER PACKAGE: Finished movement."); }