diff --git a/snp_motion_planning/src/planning_server.cpp b/snp_motion_planning/src/planning_server.cpp index 5fac13ab..b2f4a651 100644 --- a/snp_motion_planning/src/planning_server.cpp +++ b/snp_motion_planning/src/planning_server.cpp @@ -237,7 +237,7 @@ class PlanningServer // Advertise the ROS2 service server_ = node_->create_service( - PLANNING_SERVICE, std::bind(&PlanningServer::plan, this, std::placeholders::_1, std::placeholders::_2)); + PLANNING_SERVICE, std::bind(&PlanningServer::planCallback, this, std::placeholders::_1, std::placeholders::_2)); remove_scan_link_server_ = node_->create_service( REMOVE_SCAN_LINK_SERVICE, std::bind(&PlanningServer::removeScanLinkCallback, this, std::placeholders::_1, std::placeholders::_2)); @@ -330,92 +330,206 @@ class PlanningServer env_->applyCommand(std::make_shared(SCAN_LINK_NAME)); } + void addScanLink(const std::string& mesh_filename, const std::string& mesh_frame) + { + // Add the scan as a collision object to the environment + { + // Remove any previously added collision object + removeScanLink(); + + auto collision_object_type = get(node_, COLLISION_OBJECT_TYPE_PARAM); + std::vector collision_objects; + if (collision_object_type == "convex_mesh") + collision_objects = scanMeshToConvexMesh(mesh_filename); + else if (collision_object_type == "mesh") + collision_objects = scanMeshToMesh(mesh_filename); + else if (collision_object_type == "octree") + { + double octree_resolution = get(node_, OCTREE_RESOLUTION_PARAM); + if (octree_resolution < std::numeric_limits::epsilon()) + throw std::runtime_error("Octree resolution must be > 0.0"); + collision_objects = { scanMeshToOctree(mesh_filename, octree_resolution) }; + } + else + { + std::stringstream ss; + ss << "Invalid collision object type (" << collision_object_type + << ") for adding scan mesh to planning environment. Supported types are 'convex_mesh', 'mesh', and " + "'octree'"; + throw std::runtime_error(ss.str()); + } + + tesseract_environment::Commands env_cmds = createScanAdditionCommands( + collision_objects, mesh_frame, get>(node_, SCAN_DISABLED_CONTACT_LINKS)); + + env_->applyCommands(env_cmds); + } + } + void removeScanLinkCallback(const std_srvs::srv::Empty::Request::SharedPtr, std_srvs::srv::Empty::Response::SharedPtr) { removeScanLink(); } - void plan(const snp_msgs::srv::GenerateMotionPlan::Request::SharedPtr req, - snp_msgs::srv::GenerateMotionPlan::Response::SharedPtr res) + tesseract_planning::ProfileDictionary::Ptr createProfileDictionary() { - try + tesseract_planning::ProfileDictionary::Ptr profile_dict = std::make_shared(); + + // Add custom profiles { - RCLCPP_INFO_STREAM(node_->get_logger(), "Received motion planning request"); + // Get the default minimum distance allowable between any two links + auto min_contact_dist = get(node_, MIN_CONTACT_DIST_PARAM); - tesseract_planning::ProfileDictionary::Ptr profile_dict = - std::make_shared(); + // Create a list of collision pairs between the scan link and the specified links where the minimum contact + // distance is 0.0, rather than `min_contact_dist` The assumption is that these links are anticipated to come + // very close to the scan but still should not contact it + std::vector collision_pairs; + { + auto scan_contact_links = get>(node_, SCAN_REDUCED_CONTACT_LINKS_PARAM); + for (const std::string& link : scan_contact_links) + collision_pairs.emplace_back(link, SCAN_LINK_NAME, 0.0); + } + + profile_dict->addProfile(SIMPLE_DEFAULT_NAMESPACE, PROFILE, + createSimplePlannerProfile()); + { + auto profile = createOMPLProfile(min_contact_dist, collision_pairs); + profile->planning_time = get(node_, OMPL_MAX_PLANNING_TIME_PARAM); + profile_dict->addProfile(OMPL_DEFAULT_NAMESPACE, PROFILE, profile); + } + profile_dict->addProfile(TRAJOPT_DEFAULT_NAMESPACE, PROFILE, + createTrajOptToolZFreePlanProfile()); + profile_dict->addProfile( + TRAJOPT_DEFAULT_NAMESPACE, PROFILE, createTrajOptProfile(min_contact_dist, collision_pairs)); + profile_dict->addProfile>( + DESCARTES_DEFAULT_NAMESPACE, PROFILE, createDescartesPlanProfile(min_contact_dist, collision_pairs)); + profile_dict->addProfile( + MIN_LENGTH_DEFAULT_NAMESPACE, PROFILE, std::make_shared(6)); + auto velocity_scaling_factor = + clamp(get(node_, VEL_SCALE_PARAM), std::numeric_limits::epsilon(), 1.0); + auto acceleration_scaling_factor = + clamp(get(node_, ACC_SCALE_PARAM), std::numeric_limits::epsilon(), 1.0); + + // ISP profile + profile_dict->addProfile( + ISP_DEFAULT_NAMESPACE, PROFILE, + std::make_shared(velocity_scaling_factor, + acceleration_scaling_factor)); + + // Discrete contact check profile + auto contact_check_lvs = get(node_, LVS_PARAM); + profile_dict->addProfile( + CONTACT_CHECK_DEFAULT_NAMESPACE, PROFILE, + createContactCheckProfile(contact_check_lvs, min_contact_dist, collision_pairs)); + + // Constant TCP time parameterization profile + auto vel_trans = get(node_, MAX_TRANS_VEL_PARAM); + auto vel_rot = get(node_, MAX_ROT_VEL_PARAM); + auto acc_trans = get(node_, MAX_TRANS_ACC_PARAM); + auto acc_rot = get(node_, MAX_ROT_ACC_PARAM); + auto cart_time_param_profile = std::make_shared( + vel_trans, vel_rot, acc_trans, acc_rot, velocity_scaling_factor, acceleration_scaling_factor); + profile_dict->addProfile( + CONSTANT_TCP_SPEED_TIME_PARAM_TASK_NAME, PROFILE, cart_time_param_profile); + + // Kinematic limit check + auto check_joint_acc = get(node_, CHECK_JOINT_ACC_PARAM); + auto kin_limit_check_profile = + std::make_shared(true, true, check_joint_acc); + profile_dict->addProfile(KINEMATIC_LIMITS_CHECK_TASK_NAME, + PROFILE, kin_limit_check_profile); + + // TCP speed limit task + double tcp_max_speed = get(node_, TCP_MAX_SPEED_PARAM); // m/s + auto tcp_speed_limiter_profile = std::make_shared(tcp_max_speed); + profile_dict->addProfile(TCP_SPEED_LIMITER_TASK_NAME, PROFILE, + tcp_speed_limiter_profile); + } + + return profile_dict; + } + + tesseract_planning::CompositeInstruction plan(const tesseract_planning::CompositeInstruction& program, + tesseract_planning::ProfileDictionary::Ptr profile_dict, + const std::string& task_name) + { + // Set up task composer problem + auto task_composer_config_file = get(node_, TASK_COMPOSER_CONFIG_FILE_PARAM); + const YAML::Node task_composer_config = YAML::LoadFile(task_composer_config_file); + tesseract_planning::TaskComposerPluginFactory factory(task_composer_config); + + auto executor = factory.createTaskComposerExecutor("TaskflowExecutor"); + tesseract_planning::TaskComposerNode::UPtr task = factory.createTaskComposerNode(task_name); + if (!task) + throw std::runtime_error("Failed to create '" + task_name + "' task"); + + // Save dot graph + { + std::ofstream tc_out_data(tesseract_common::getTempPath() + task_name + ".dot"); + task->dump(tc_out_data); + } + + const std::string input_key = task->getInputKeys().front(); + const std::string output_key = task->getOutputKeys().front(); + auto task_data = std::make_shared(); + task_data->setData(input_key, program); + tesseract_planning::TaskComposerProblem::Ptr problem = + std::make_shared(env_, profile_dict); + problem->dotgraph = true; + + // Update log level for debugging + auto log_level = console_bridge::getLogLevel(); + if (get(node_, VERBOSE_PARAM)) + { + console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_DEBUG); - // Add custom profiles + // Dump dotgraphs of each task for reference + const YAML::Node& task_plugins = task_composer_config["task_composer_plugins"]["tasks"]["plugins"]; + for (auto it = task_plugins.begin(); it != task_plugins.end(); ++it) { - // Get the default minimum distance allowable between any two links - auto min_contact_dist = get(node_, MIN_CONTACT_DIST_PARAM); - - // Create a list of collision pairs between the scan link and the specified links where the minimum contact - // distance is 0.0, rather than `min_contact_dist` The assumption is that these links are anticipated to come - // very close to the scan but still should not contact it - std::vector collision_pairs; - { - auto scan_contact_links = get>(node_, SCAN_REDUCED_CONTACT_LINKS_PARAM); - for (const std::string& link : scan_contact_links) - collision_pairs.emplace_back(link, SCAN_LINK_NAME, 0.0); - } - - profile_dict->addProfile(SIMPLE_DEFAULT_NAMESPACE, PROFILE, - createSimplePlannerProfile()); - { - auto profile = createOMPLProfile(min_contact_dist, collision_pairs); - profile->planning_time = get(node_, OMPL_MAX_PLANNING_TIME_PARAM); - profile_dict->addProfile(OMPL_DEFAULT_NAMESPACE, PROFILE, profile); - } - profile_dict->addProfile(TRAJOPT_DEFAULT_NAMESPACE, PROFILE, - createTrajOptToolZFreePlanProfile()); - profile_dict->addProfile( - TRAJOPT_DEFAULT_NAMESPACE, PROFILE, createTrajOptProfile(min_contact_dist, collision_pairs)); - profile_dict->addProfile>( - DESCARTES_DEFAULT_NAMESPACE, PROFILE, createDescartesPlanProfile(min_contact_dist, collision_pairs)); - profile_dict->addProfile( - MIN_LENGTH_DEFAULT_NAMESPACE, PROFILE, std::make_shared(6)); - auto velocity_scaling_factor = - clamp(get(node_, VEL_SCALE_PARAM), std::numeric_limits::epsilon(), 1.0); - auto acceleration_scaling_factor = - clamp(get(node_, ACC_SCALE_PARAM), std::numeric_limits::epsilon(), 1.0); - - // ISP profile - profile_dict->addProfile( - ISP_DEFAULT_NAMESPACE, PROFILE, - std::make_shared(velocity_scaling_factor, - acceleration_scaling_factor)); - - // Discrete contact check profile - auto contact_check_lvs = get(node_, LVS_PARAM); - profile_dict->addProfile( - CONTACT_CHECK_DEFAULT_NAMESPACE, PROFILE, - createContactCheckProfile(contact_check_lvs, min_contact_dist, collision_pairs)); - - // Constant TCP time parameterization profile - auto vel_trans = get(node_, MAX_TRANS_VEL_PARAM); - auto vel_rot = get(node_, MAX_ROT_VEL_PARAM); - auto acc_trans = get(node_, MAX_TRANS_ACC_PARAM); - auto acc_rot = get(node_, MAX_ROT_ACC_PARAM); - auto cart_time_param_profile = - std::make_shared( - vel_trans, vel_rot, acc_trans, acc_rot, velocity_scaling_factor, acceleration_scaling_factor); - profile_dict->addProfile( - CONSTANT_TCP_SPEED_TIME_PARAM_TASK_NAME, PROFILE, cart_time_param_profile); - - // Kinematic limit check - auto check_joint_acc = get(node_, CHECK_JOINT_ACC_PARAM); - auto kin_limit_check_profile = - std::make_shared(true, true, check_joint_acc); - profile_dict->addProfile(KINEMATIC_LIMITS_CHECK_TASK_NAME, - PROFILE, kin_limit_check_profile); - - // TCP speed limit task - double tcp_max_speed = get(node_, TCP_MAX_SPEED_PARAM); // m/s - auto tcp_speed_limiter_profile = std::make_shared(tcp_max_speed); - profile_dict->addProfile(TCP_SPEED_LIMITER_TASK_NAME, PROFILE, - tcp_speed_limiter_profile); + auto task_plugin_name = it->first.as(); + std::ofstream f(tesseract_common::getTempPath() + task_plugin_name + ".dot"); + tesseract_planning::TaskComposerNode::Ptr task = factory.createTaskComposerNode(task_plugin_name); + if (!task) + throw std::runtime_error("Failed to load task: '" + task_plugin_name + "'"); + task->dump(f); } + } + + // Run problem + tesseract_planning::TaskComposerFuture::UPtr result = executor->run(*task, problem, task_data); + result->wait(); + + // Save the output dot graph + { + std::ofstream tc_out_results(tesseract_common::getTempPath() + task_name + "_results.dot"); + static_cast(*task).dump(tc_out_results, nullptr, + result->context->task_infos.getInfoMap()); + } + + // Reset the log level + console_bridge::setLogLevel(log_level); + + // Check for successful plan + if (!result->context->isSuccessful() || result->context->isAborted()) + throw std::runtime_error("Failed to create motion plan"); + + // Get results of successful plan + tesseract_planning::CompositeInstruction program_results = + result->context->data_storage->getData(output_key).as(); + + // Send joint trajectory to Tesseract plotter widget + plotter_->plotTrajectory(toJointTrajectory(program_results), *env_->getStateSolver()); + + return program_results; + } + + void planCallback(const snp_msgs::srv::GenerateMotionPlan::Request::SharedPtr req, + snp_msgs::srv::GenerateMotionPlan::Response::SharedPtr res) + { + try + { + RCLCPP_INFO_STREAM(node_->get_logger(), "Received motion planning request"); // Create a manipulator info and program from the service request const std::string& base_frame = req->tool_paths.at(0).segments.at(0).header.frame_id; @@ -436,104 +550,13 @@ class PlanningServer // Set up composite instruction and environment tesseract_planning::CompositeInstruction program = createProgram(manip_info, fromMsg(req->tool_paths)); - // Add the scan as a collision object to the environment - { - // Remove any previously added collision object - removeScanLink(); - - auto collision_object_type = get(node_, COLLISION_OBJECT_TYPE_PARAM); - std::vector collision_objects; - if (collision_object_type == "convex_mesh") - collision_objects = scanMeshToConvexMesh(req->mesh_filename); - else if (collision_object_type == "mesh") - collision_objects = scanMeshToMesh(req->mesh_filename); - else if (collision_object_type == "octree") - { - double octree_resolution = get(node_, OCTREE_RESOLUTION_PARAM); - if (octree_resolution < std::numeric_limits::epsilon()) - throw std::runtime_error("Octree resolution must be > 0.0"); - collision_objects = { scanMeshToOctree(req->mesh_filename, octree_resolution) }; - } - else - { - std::stringstream ss; - ss << "Invalid collision object type (" << collision_object_type - << ") for adding scan mesh to planning environment. Supported types are 'convex_mesh', 'mesh', and " - "'octree'"; - throw std::runtime_error(ss.str()); - } - - tesseract_environment::Commands env_cmds = createScanAdditionCommands( - collision_objects, req->mesh_frame, get>(node_, SCAN_DISABLED_CONTACT_LINKS)); - - env_->applyCommands(env_cmds); - } - - // Set up task composer problem - auto task_composer_config_file = get(node_, TASK_COMPOSER_CONFIG_FILE_PARAM); - const YAML::Node task_composer_config = YAML::LoadFile(task_composer_config_file); - tesseract_planning::TaskComposerPluginFactory factory(task_composer_config); + // Add the scan link to the planning environment + addScanLink(req->mesh_filename, req->mesh_frame); + // Invoke the planner + auto pd = createProfileDictionary(); auto task_name = get(node_, TASK_NAME_PARAM); - auto executor = factory.createTaskComposerExecutor("TaskflowExecutor"); - tesseract_planning::TaskComposerNode::UPtr task = factory.createTaskComposerNode(task_name); - if (!task) - throw std::runtime_error("Failed to create '" + task_name + "' task"); - - // Save dot graph - { - std::ofstream tc_out_data(tesseract_common::getTempPath() + task_name + ".dot"); - task->dump(tc_out_data); - } - - const std::string input_key = task->getInputKeys().front(); - const std::string output_key = task->getOutputKeys().front(); - auto task_data = std::make_shared(); - task_data->setData(input_key, program); - tesseract_planning::TaskComposerProblem::Ptr problem = - std::make_shared(env_, profile_dict); - problem->dotgraph = true; - - // Update log level for debugging - auto log_level = console_bridge::getLogLevel(); - if (get(node_, VERBOSE_PARAM)) - { - console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_DEBUG); - - // Dump dotgraphs of each task for reference - const YAML::Node& task_plugins = task_composer_config["task_composer_plugins"]["tasks"]["plugins"]; - for (auto it = task_plugins.begin(); it != task_plugins.end(); ++it) - { - auto task_plugin_name = it->first.as(); - std::ofstream f(tesseract_common::getTempPath() + task_plugin_name + ".dot"); - tesseract_planning::TaskComposerNode::Ptr task = factory.createTaskComposerNode(task_plugin_name); - if (!task) - throw std::runtime_error("Failed to load task: '" + task_plugin_name + "'"); - task->dump(f); - } - } - - // Run problem - tesseract_planning::TaskComposerFuture::UPtr result = executor->run(*task, problem, task_data); - result->wait(); - - // Save the output dot graph - { - std::ofstream tc_out_results(tesseract_common::getTempPath() + task_name + "_results.dot"); - static_cast(*task).dump(tc_out_results, nullptr, - result->context->task_infos.getInfoMap()); - } - - // Reset the log level - console_bridge::setLogLevel(log_level); - - // Check for successful plan - if (!result->context->isSuccessful() || result->context->isAborted()) - throw std::runtime_error("Failed to create motion plan"); - - // Get results of successful plan - tesseract_planning::CompositeInstruction program_results = - result->context->data_storage->getData(output_key).as(); + tesseract_planning::CompositeInstruction program_results = plan(program, pd, task_name); if (program_results.size() < 3) { @@ -544,9 +567,6 @@ class PlanningServer throw std::runtime_error(ss.str()); } - // Send joint trajectory to Tesseract plotter widget - plotter_->plotTrajectory(toJointTrajectory(program_results), *env_->getStateSolver()); - // Return results res->approach = tesseract_rosutils::toMsg(toJointTrajectory(*program_results.begin()), env_->getState());