From 934d7f6b8ee5173a39edf3a32393415f6300447b Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Tue, 16 Apr 2024 18:19:13 -0500 Subject: [PATCH 1/6] WIP update organization of planner code --- snp_motion_planning/src/planning_server.cpp | 373 +++++++++++--------- 1 file changed, 199 insertions(+), 174 deletions(-) diff --git a/snp_motion_planning/src/planning_server.cpp b/snp_motion_planning/src/planning_server.cpp index 5fac13ab..9f0b7e23 100644 --- a/snp_motion_planning/src/planning_server.cpp +++ b/snp_motion_planning/src/planning_server.cpp @@ -330,92 +330,209 @@ 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(tesseract_planning::CompositeInstruction& program, + tesseract_planning::ProfileDictionary& profile_dict, + 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 +553,15 @@ 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); - - 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()); - } + // Add the scan link to the planning environment + addScanLink(req->mesh_filename, req->mesh_frame); - // Reset the log level - console_bridge::setLogLevel(log_level); + // Invoke the planner + auto pd = createProfileDictionary(); + tesseract_planning::CompositeInstruction program_results = plan(program, *pd, get_parameter(node_, TASK_NAME_PARAM)); - // 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(); + // Remove scan link? + removeScanLink(); if (program_results.size() < 3) { @@ -544,9 +572,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()); From e7255412a8a2342a960f8380af0a7610ac4907c7 Mon Sep 17 00:00:00 2001 From: David Spielman Date: Wed, 17 Apr 2024 11:20:32 -0500 Subject: [PATCH 2/6] Reorganized planner_server.cpp to simplify motion plan generations --- snp_motion_planning/launch/planning_server.launch.xml | 4 ++-- snp_motion_planning/src/planning_server.cpp | 11 ++++++----- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/snp_motion_planning/launch/planning_server.launch.xml b/snp_motion_planning/launch/planning_server.launch.xml index 6c136c21..04b80a7e 100644 --- a/snp_motion_planning/launch/planning_server.launch.xml +++ b/snp_motion_planning/launch/planning_server.launch.xml @@ -12,7 +12,7 @@ - + @@ -38,7 +38,7 @@ - + diff --git a/snp_motion_planning/src/planning_server.cpp b/snp_motion_planning/src/planning_server.cpp index 9f0b7e23..4148dd99 100644 --- a/snp_motion_planning/src/planning_server.cpp +++ b/snp_motion_planning/src/planning_server.cpp @@ -47,7 +47,7 @@ static const std::string COLLISION_OBJECT_TYPE_PARAM = "collision_object_type"; static const std::string OCTREE_RESOLUTION_PARAM = "octree_resolution"; // Task composer static const std::string TASK_COMPOSER_CONFIG_FILE_PARAM = "task_composer_config_file"; -static const std::string TASK_NAME_PARAM = "task_name"; +static const std::string RASTER_TASK_NAME_PARAM = "raster_task_name"; // Profile static const std::string MAX_TRANS_VEL_PARAM = "max_translational_vel"; static const std::string MAX_ROT_VEL_PARAM = "max_rotational_vel"; @@ -215,7 +215,7 @@ class PlanningServer // Task composer node_->declare_parameter(TASK_COMPOSER_CONFIG_FILE_PARAM, ""); - node_->declare_parameter(TASK_NAME_PARAM, ""); + node_->declare_parameter(RASTER_TASK_NAME_PARAM, ""); { auto urdf_string = get(node_, "robot_description"); @@ -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)); @@ -453,7 +453,7 @@ class PlanningServer } tesseract_planning::CompositeInstruction plan(tesseract_planning::CompositeInstruction& program, - tesseract_planning::ProfileDictionary& profile_dict, + tesseract_planning::ProfileDictionary::Ptr& profile_dict, std::string& task_name) { // Set up task composer problem @@ -558,7 +558,8 @@ class PlanningServer // Invoke the planner auto pd = createProfileDictionary(); - tesseract_planning::CompositeInstruction program_results = plan(program, *pd, get_parameter(node_, TASK_NAME_PARAM)); + auto raster_task_name = get(node_, RASTER_TASK_NAME_PARAM); + tesseract_planning::CompositeInstruction program_results = plan(program, pd, raster_task_name); // Remove scan link? removeScanLink(); From f29c4dc130680161242864aff88076cde1a488a3 Mon Sep 17 00:00:00 2001 From: David Spielman Date: Wed, 17 Apr 2024 11:39:11 -0500 Subject: [PATCH 3/6] Ran clang formatting --- snp_motion_planning/src/planning_server.cpp | 33 ++++++++++----------- 1 file changed, 15 insertions(+), 18 deletions(-) diff --git a/snp_motion_planning/src/planning_server.cpp b/snp_motion_planning/src/planning_server.cpp index 4148dd99..21dadb5e 100644 --- a/snp_motion_planning/src/planning_server.cpp +++ b/snp_motion_planning/src/planning_server.cpp @@ -330,8 +330,7 @@ class PlanningServer env_->applyCommand(std::make_shared(SCAN_LINK_NAME)); } - void addScanLink(const std::string& mesh_filename, - const std::string& mesh_frame) + void addScanLink(const std::string& mesh_filename, const std::string& mesh_frame) { // Add the scan as a collision object to the environment { @@ -374,17 +373,16 @@ class PlanningServer tesseract_planning::ProfileDictionary::Ptr createProfileDictionary() { - tesseract_planning::ProfileDictionary::Ptr profile_dict = - std::make_shared(); + tesseract_planning::ProfileDictionary::Ptr profile_dict = std::make_shared(); // Add custom profiles { // 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 + // 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); @@ -412,37 +410,36 @@ class PlanningServer auto acceleration_scaling_factor = clamp(get(node_, ACC_SCALE_PARAM), std::numeric_limits::epsilon(), 1.0); - // ISP profile + // ISP profile profile_dict->addProfile( ISP_DEFAULT_NAMESPACE, PROFILE, std::make_shared(velocity_scaling_factor, acceleration_scaling_factor)); - // Discrete contact check profile + // 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 + // 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); + 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 + // 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 + // 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, @@ -466,7 +463,7 @@ class PlanningServer if (!task) throw std::runtime_error("Failed to create '" + task_name + "' task"); - // Save dot graph + // Save dot graph { std::ofstream tc_out_data(tesseract_common::getTempPath() + task_name + ".dot"); task->dump(tc_out_data); @@ -480,13 +477,13 @@ class PlanningServer std::make_shared(env_, profile_dict); problem->dotgraph = true; - // Update log level for debugging + // 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 + // 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) { From c745188fa0624db234c66b3d8e7f82b6990a39ee Mon Sep 17 00:00:00 2001 From: David Spielman Date: Fri, 3 May 2024 18:05:03 -0500 Subject: [PATCH 4/6] Reverted name of raster task name to task name --- snp_motion_planning/launch/planning_server.launch.xml | 4 ++-- snp_motion_planning/src/planning_server.cpp | 8 ++++---- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/snp_motion_planning/launch/planning_server.launch.xml b/snp_motion_planning/launch/planning_server.launch.xml index 04b80a7e..6c136c21 100644 --- a/snp_motion_planning/launch/planning_server.launch.xml +++ b/snp_motion_planning/launch/planning_server.launch.xml @@ -12,7 +12,7 @@ - + @@ -38,7 +38,7 @@ - + diff --git a/snp_motion_planning/src/planning_server.cpp b/snp_motion_planning/src/planning_server.cpp index 21dadb5e..64296ba9 100644 --- a/snp_motion_planning/src/planning_server.cpp +++ b/snp_motion_planning/src/planning_server.cpp @@ -47,7 +47,7 @@ static const std::string COLLISION_OBJECT_TYPE_PARAM = "collision_object_type"; static const std::string OCTREE_RESOLUTION_PARAM = "octree_resolution"; // Task composer static const std::string TASK_COMPOSER_CONFIG_FILE_PARAM = "task_composer_config_file"; -static const std::string RASTER_TASK_NAME_PARAM = "raster_task_name"; +static const std::string TASK_NAME_PARAM = "task_name"; // Profile static const std::string MAX_TRANS_VEL_PARAM = "max_translational_vel"; static const std::string MAX_ROT_VEL_PARAM = "max_rotational_vel"; @@ -215,7 +215,7 @@ class PlanningServer // Task composer node_->declare_parameter(TASK_COMPOSER_CONFIG_FILE_PARAM, ""); - node_->declare_parameter(RASTER_TASK_NAME_PARAM, ""); + node_->declare_parameter(TASK_NAME_PARAM, ""); { auto urdf_string = get(node_, "robot_description"); @@ -555,8 +555,8 @@ class PlanningServer // Invoke the planner auto pd = createProfileDictionary(); - auto raster_task_name = get(node_, RASTER_TASK_NAME_PARAM); - tesseract_planning::CompositeInstruction program_results = plan(program, pd, raster_task_name); + auto task_name = get(node_, TASK_NAME_PARAM); + tesseract_planning::CompositeInstruction program_results = plan(program, pd, task_name); // Remove scan link? removeScanLink(); From 8fefc30dd03cd57dbeab90d20649d6b8b02a4ee7 Mon Sep 17 00:00:00 2001 From: David Spielman Date: Fri, 3 May 2024 18:26:00 -0500 Subject: [PATCH 5/6] Modified signature of plan method --- snp_motion_planning/src/planning_server.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/snp_motion_planning/src/planning_server.cpp b/snp_motion_planning/src/planning_server.cpp index 64296ba9..84215ec6 100644 --- a/snp_motion_planning/src/planning_server.cpp +++ b/snp_motion_planning/src/planning_server.cpp @@ -449,9 +449,9 @@ class PlanningServer return profile_dict; } - tesseract_planning::CompositeInstruction plan(tesseract_planning::CompositeInstruction& program, - tesseract_planning::ProfileDictionary::Ptr& profile_dict, - std::string& task_name) + 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); From 6a1e387b5189f23d452ee96639e1489fdc5bb3e0 Mon Sep 17 00:00:00 2001 From: David Spielman Date: Fri, 3 May 2024 18:28:07 -0500 Subject: [PATCH 6/6] Removed call to remove scan link in planCallback method --- snp_motion_planning/src/planning_server.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/snp_motion_planning/src/planning_server.cpp b/snp_motion_planning/src/planning_server.cpp index 84215ec6..b2f4a651 100644 --- a/snp_motion_planning/src/planning_server.cpp +++ b/snp_motion_planning/src/planning_server.cpp @@ -558,9 +558,6 @@ class PlanningServer auto task_name = get(node_, TASK_NAME_PARAM); tesseract_planning::CompositeInstruction program_results = plan(program, pd, task_name); - // Remove scan link? - removeScanLink(); - if (program_results.size() < 3) { std::stringstream ss;