diff --git a/src/lite6_controller/src/lite6_controller.cpp b/src/lite6_controller/src/lite6_controller.cpp index 2616d41..d21403b 100644 --- a/src/lite6_controller/src/lite6_controller.cpp +++ b/src/lite6_controller/src/lite6_controller.cpp @@ -47,15 +47,6 @@ const std::string MOVE_GROUP = "lite6"; using namespace std::chrono_literals; -// MOTION PLANNING API -// https://github.com/ros-planning/moveit2_tutorials/blob/humble/doc/examples/motion_planning_api/src/motion_planning_api_tutorial.cpp -// https://moveit.picknik.ai/humble/doc/examples/motion_planning_api/motion_planning_api_tutorial.html -// https://github.com/ros-planning/moveit2/blob/main/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_service.cpp -// -// -// USE -// https://industrial-training-master.readthedocs.io/en/foxy/_source/session4/ros2/0-Motion-Planning-CPP.html - /** * Controller for xArm Lite6, implementing RobotController */ @@ -92,17 +83,6 @@ public: float zlim_lower = 0.1945; float zlim_upper = 0.200; - //bool moved = false; - // - // TODO get pilz working - //std::unique_ptr command_list_manager_; - // command_list_manager_ = std::make_unique(this->move_group.getNodeHandle(), this->move_group.getRobotModel()); - - /** - * CommandListManager, used to plan MotionSequenceRequest - */ - //pilz_industrial_motion_planner::CommandListManager command_list_manager; - //pilz_industrial_motion_planner::CommandListManager command_list_manager(*std::shared_ptr(std::move(this)), this->move_group.getRobotModel()); /** * Constructor @@ -110,24 +90,11 @@ public: Lite6Controller(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) : RobotController(options), move_group(std::shared_ptr(std::move(this)), MOVE_GROUP) - //moveit_cpp_(std::shared_ptr(std::move(this))), - //planning_component_(MOVE_GROUP, moveit_cpp_), - //command_list_manager(std::shared_ptr(std::move(this)), this->move_group.getRobotModel()) { - - //command_list_manager = new pilz_industrial_motion_planner::CommandListManager(this->move_group.getNodeHandle(), this->move_group.getRobotModel()); - //command_list_manager = new pilz_industrial_motion_planner::CommandListManager(std::shared_ptr(std::move(this)), this->move_group.getRobotModel()); - // Use upper joint velocity and acceleration limits this->move_group.setMaxAccelerationScalingFactor(1.0); - - //setting this lower seems to improve overall time and prevents robot from moving too fast this->move_group.setMaxVelocityScalingFactor(1.0); //this->move_group.setMaxVelocityScalingFactor(0.8); - // Subscribe to target pose - //target_pose_sub_ = this->create_subscription("/target_pose", rclcpp::QoS(1), std::bind(&MoveItFollowTarget::target_pose_callback, this, std::placeholders::_1)); - - // trajectory_timer_ = this->create_wall_timer( 10ms, std::bind(&Lite6Controller::executeTrajectoryFromQueue, this)); @@ -135,18 +102,10 @@ public: void setup() { - //moveit_cpp_ = std::make_shared(move_group.getNodeHandle()); try { - //moveit_cpp_ = std::make_shared(this->shared_from_this()); - //moveit_cpp_ = std::make_shared(move_group.getNodeHandle()); - //planning_component_ = std::make_shared(MOVE_GROUP, moveit_cpp_); - // - planning_scene_monitor_ = std::make_shared(this->shared_from_this(), "robot_description"); - //RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Getting initial robot state"); - //move_group.startStateMonitor(5); this->sequence_client_ = this->create_client("plan_sequence_path"); @@ -217,11 +176,12 @@ public: /** * */ - moveit_msgs::msg::MotionSequenceRequest createMotionSequenceRequest(const std::vector *path) + moveit_msgs::msg::MotionSequenceRequest createMotionSequenceRequest(const std::vector *path, std::string planner_id) { moveit_msgs::msg::MotionSequenceRequest msr = moveit_msgs::msg::MotionSequenceRequest(); //waypoints.push_back(move_group.getCurrentPose().pose); std::string ee_link = move_group.getLinkNames().back(); + //std::string ee_link = "pen_link"; RCLCPP_INFO(this->get_logger(), "Got ee_link: %s", ee_link.c_str()); geometry_msgs::msg::Point previous_point; //previous_point.point.x = -1.0; @@ -233,14 +193,15 @@ public: moveit_msgs::msg::MotionSequenceItem msi = moveit_msgs::msg::MotionSequenceItem(); planning_interface::MotionPlanRequest mpr = planning_interface::MotionPlanRequest(); + mpr.planner_id = planner_id; //mpr.planner_id = "PTP"; - mpr.planner_id = "LIN"; + //mpr.planner_id = "LIN"; mpr.group_name = move_group.getName(); //mpr.max_velocity_scaling_factor = 1.0; mpr.max_velocity_scaling_factor = 0.3; - mpr.max_acceleration_scaling_factor = 0.4; + mpr.max_acceleration_scaling_factor = 0.3; //mpr.max_acceleration_scaling_factor = 0.1; - mpr.allowed_planning_time = 10; + mpr.allowed_planning_time = 20; mpr.max_cartesian_speed = 3; // m/s //mpr.goal_constraints.position_constraints.header.frame_id = "world"; @@ -299,6 +260,11 @@ public: return msr; } + moveit_msgs::msg::MotionSequenceRequest createMotionSequenceRequest(const std::vector *path) + { + return createMotionSequenceRequest(path, "LIN"); + } + /** * */ @@ -411,6 +377,103 @@ public: busy = false; } + moveit_msgs::msg::RobotTrajectory sendRequest(moveit_msgs::msg::MotionSequenceRequest msr) + { + auto req = std::make_shared(); + req->request = msr; + RCLCPP_INFO(this->get_logger(), "Sending request to sequence service"); + auto res = sequence_client_->async_send_request(req); + + auto ts = res.get()->response.planned_trajectories; + + // Set move_group_state to the last state of planned trajectory (planning of next trajectory starts there) + + if (ts.empty()) + { + moveit_msgs::msg::RobotTrajectory t; + return t; + } + + + return ts[0]; + } + + void setMoveGroupState(moveit_msgs::msg::RobotTrajectory t) + { + + if (t.joint_trajectory.points.empty()) + { + RCLCPP_ERROR(this->get_logger(), "TRAJECTORY IS EMPTY"); + return; + } + + robot_trajectory::RobotTrajectory rt(move_group.getRobotModel()); + rt.setRobotTrajectoryMsg(*move_group_state, t); + move_group_state = rt.getLastWayPointPtr(); + } + + void pathToTrajectory(const std::vector *path, std::vector *ts) + { + std::vector penup = {}; + std::vector tail = {}; + bool up = true; + if (!path) + { + RCLCPP_ERROR(this->get_logger(), "Received null pointer for path"); + return; + } + for (auto p : *path) + { + if (p.pose.position.z > 0) + up = false; + if (up) + //penup->push_back(p); + penup.push_back(p); + else + { + up = false; + //tail->push_back(p); + tail.push_back(p); + } + } + + if (!penup.empty()) + { + auto msr = createMotionSequenceRequest(&penup, "PTP"); + RCLCPP_ERROR(this->get_logger(), "Created MSR for penup"); + if (msr.items.empty()) + { + RCLCPP_ERROR(this->get_logger(), "MSR EMPTY"); + return; + } + ts->push_back(sendRequest(msr)); + //planAndLogOffset(&penup); + setMoveGroupState(ts->back()); + + } + else + { + RCLCPP_ERROR(this->get_logger(), "Penup path is empty, all motions will be LIN"); + } + if (!tail.empty()) + { + auto msr = createMotionSequenceRequest(&tail, "LIN"); + RCLCPP_ERROR(this->get_logger(), "Created MSR for tail"); + if (msr.items.empty()) + { + RCLCPP_ERROR(this->get_logger(), "MSR EMPTY"); + return; + } + ts->push_back(sendRequest(msr)); + //planAndLogOffset(&tail); + setMoveGroupState(ts->back()); + } + else + { + RCLCPP_ERROR(this->get_logger(), "Path was empty, no trajectories created"); + } + } + /** * Callback that executes path on robot */ @@ -423,37 +486,33 @@ public: auto feedback = std::make_shared(); auto result = std::make_shared(); - auto msr = createMotionSequenceRequest(&goal->motion.path); - RCLCPP_INFO(this->get_logger(), "Created MSR"); + std::vector ts; + pathToTrajectory(&goal->motion.path, &ts); - //planAndLogOffset(&goal->motion.path); + long unsigned int n = 0; + + RCLCPP_INFO(this->get_logger(), "Got %ld trajectories", ts.size()); + for (auto t : ts) + { + RCLCPP_INFO(this->get_logger(), "Adding result to motion queue"); + if (t.joint_trajectory.points.empty()) + { + RCLCPP_ERROR(this->get_logger(), "TRAJECTORY IS EMPTY"); + continue; + } + trajectory_queue.push(t); + n++; + } - RCLCPP_INFO(this->get_logger(), "Creating req"); - auto req = std::make_shared(); - RCLCPP_INFO(this->get_logger(), "Setting msr request"); - req->request = msr; - RCLCPP_INFO(this->get_logger(), "Sending request to sequence service"); - auto res = sequence_client_->async_send_request(req); - RCLCPP_INFO(this->get_logger(), "Waiting for result"); - auto ts = res.get()->response.planned_trajectories; std::string status = ""; - if (ts.size() > 0) + if (n == ts.size()) { status = "success"; - RCLCPP_INFO(this->get_logger(), "Got %ld trajectories", ts.size()); - RCLCPP_INFO(this->get_logger(), "Adding result to motion queue"); - - trajectory_queue.push(ts[0]); - - // Set move_group_state to the last state of planned trajectory (planning of next trajectory starts there) - robot_trajectory::RobotTrajectory rt(move_group.getRobotModel()); - rt.setRobotTrajectoryMsg(*move_group_state, ts[0]); - move_group_state = rt.getLastWayPointPtr(); - status = status + "," + pointsToString(&goal->motion.path,0,0,0); appendLineToFile("OUTPUT.csv", status); result->result = "success"; + goal_handle->succeed(result); RCLCPP_INFO(this->get_logger(), "Goal succeeded"); return;