diff --git a/src/ar3_config/launch/dev.launch.py b/src/ar3_config/launch/dev.launch.py index ac11a5b..dc1ee54 100644 --- a/src/ar3_config/launch/dev.launch.py +++ b/src/ar3_config/launch/dev.launch.py @@ -54,7 +54,9 @@ def generate_launch_description(): get_yaml(join(config_root, 'ompl_planning.yaml')), get_yaml(join(config_root, 'trajectory.yaml')), get_yaml(join(config_root, 'planning_scene.yaml')), - get_yaml(join(config_root, "ar3_controllers.yaml"))], + get_yaml(join(config_root, "ar3_controllers.yaml")), + { "default_planning_pipeline": "ompl" }, + { "planning_pipelines": {"pipeline_names":["ompl"], "ompl": {"planning_plugin": "ompl_interface/OMPLPlanner"} } }] ), # Our joystick node @@ -133,7 +135,7 @@ def generate_launch_description(): get_yaml(join(config_root, 'planning_scene.yaml')), get_yaml(join(config_root, "ar3_controllers.yaml")), { "default_planning_pipeline": "move_group" }, - { "planning_pipelines": [ "ompl", "move_group"] } + { "planning_pipelines": {"pipeline_names":["ompl"], "ompl": {"planning_plugin": "ompl_interface/OMPLPlanner"} } } ], ), diff --git a/src/cobot_platform/cobot_platform/move.py b/src/cobot_platform/cobot_platform/move.py index 8ccb1b1..a84cccf 100644 --- a/src/cobot_platform/cobot_platform/move.py +++ b/src/cobot_platform/cobot_platform/move.py @@ -23,7 +23,7 @@ class CobotMove(Node): self._cur = [0.0] * 6 # create a timer to update all the joint angles at once - self._timer = self.create_timer(1 / 40, self._dosomething) + self._timer = self.create_timer(1 / 20, self._dosomething) # Set joint angles independantly base_topic = 'move/' diff --git a/src/move_group_interface/src/move_group_interface_node.cpp b/src/move_group_interface/src/move_group_interface_node.cpp index d024a0a..3da082c 100644 --- a/src/move_group_interface/src/move_group_interface_node.cpp +++ b/src/move_group_interface/src/move_group_interface_node.cpp @@ -5,8 +5,9 @@ #include // MoveitCpp -#include -#include +#include +#include +#include #include "geometry_msgs/msg/pose_stamped.hpp" @@ -15,66 +16,66 @@ const std::string PLANNING_GROUP = "ar3_arm"; class ScanNPlan : public rclcpp::Node { public: ScanNPlan() : Node("scan_n_plan") { - RCLCPP_INFO(this->get_logger(), "SNP START"); - // Create /move_pose subscriber pose_sub_ = this->create_subscription( "move_pose", rclcpp::QoS(1), std::bind(&ScanNPlan::moveCallback, this, std::placeholders::_1) ); - RCLCPP_INFO(this->get_logger(), "SUB EXIST"); + + relative_pose_sub_ = this->create_subscription( + "move_relative_pose", + rclcpp::QoS(1), + std::bind(&ScanNPlan::relativeMoveCallback, this, std::placeholders::_1) + ); + } + + void init() { + move_group_ = std::make_shared( + shared_from_this(), std::string("ar3_arm")); } private: void moveCallback(geometry_msgs::msg::PoseStamped::SharedPtr p) { - (void) p; RCLCPP_INFO(this->get_logger(), "GOT MSG"); - // Get a shared pointer to the MoveItCpp library to use our planning scene and getting states - auto moveit_cpp_ptr = std::make_shared(shared_from_this()); - RCLCPP_INFO(this->get_logger(), "GOT MOVEIT"); - //auto psm = std::make_shared("robot_description"); - //psm->startSceneMonitor(); + move_group_->setPoseTarget(p->pose); - // ?????? - moveit_cpp_ptr->getPlanningSceneMonitor()->providePlanningSceneService(); + moveit::planning_interface::MoveGroupInterface::Plan plan; + bool success = (move_group_->plan(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); + RCLCPP_INFO(this->get_logger(), "Did work? %s", success ? "yes" : "no"); - // planning_scene_monitor::LockedPlanningSceneRW scene{moveit_cpp_ptr->getPlanningSceneMonitor()}; - RCLCPP_INFO(this->get_logger(), "NEW PLANNING"); + move_group_->move(); + } - auto planning_components = std::make_shared(PLANNING_GROUP, moveit_cpp_ptr); - RCLCPP_INFO(this->get_logger(), "GET MODEL"); - auto robot_model_ptr = moveit_cpp_ptr->getRobotModel(); - RCLCPP_INFO(this->get_logger(), "GET START STATE"); - auto robot_start_state = planning_components->getStartState(); + void relativeMoveCallback(geometry_msgs::msg::PoseStamped::SharedPtr p) { + RCLCPP_INFO(this->get_logger(), "GOT MSG"); - RCLCPP_INFO(this->get_logger(), "SETSTART TO CUR"); - planning_components->setStartStateToCurrentState(); + moveit::core::RobotStatePtr robot_state = move_group_->getCurrentState(); + const Eigen::Isometry3d &end_effector_state = robot_state->getGlobalLinkTransform(move_group_->getEndEffectorLink()); + RCLCPP_INFO_STREAM(this->get_logger(), "POS: \n" << end_effector_state.translation() << "\n"); + RCLCPP_INFO_STREAM(this->get_logger(), "ROT: \n" << end_effector_state.rotation() << "\n"); - // Set the goal to the request pose - RCLCPP_INFO(this->get_logger(), "SET GOAL"); - planning_components->setGoal(*p, "base_link"); + geometry_msgs::msg::Pose target_pose; + target_pose.position.x = 0.24; + target_pose.position.y = -0.32; + target_pose.position.z = 0.56; + move_group_->setPoseTarget(target_pose); - // Actually plan - RCLCPP_INFO(this->get_logger(), "DO PLAN"); - auto plan_solution = planning_components->plan(); + moveit::planning_interface::MoveGroupInterface::Plan plan; + bool success = (move_group_->plan(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); - // Plan is valid - if (plan_solution) { - // Follow through with the planned path - planning_components->execute(); - } + RCLCPP_INFO(this->get_logger(), "Did work? %s", success ? "yes" : "no"); + + move_group_->move(); } private: rclcpp::Subscription::SharedPtr pose_sub_; + rclcpp::Subscription::SharedPtr relative_pose_sub_; - //please - //rclcpp::Node::SharedPtr nh_; - // moveit_cpp::MoveItCpp::SharedPtr moveit_cpp_ptr_; - // rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; - // std::thread thread_executor_spin_; + moveit::planning_interface::MoveGroupInterfacePtr move_group_; + moveit::core::RobotModelConstPtr robot_model_; }; int main(int argc, char* argv[]) { @@ -83,12 +84,9 @@ int main(int argc, char* argv[]) { auto node = std::make_shared(); RCLCPP_INFO(node->get_logger(), "MoveGroup node started."); + node->init(); rclcpp::spin(node); - // spin the node - // executor_ = std::make_shared(); - // executor_->add_node(nh_); - RCLCPP_INFO(node->get_logger(), "Terminating MoveGroup"); rclcpp::shutdown(); return 0;