9 #include <choreo_msgs/ProcessPlanning.h> 10 #include <choreo_msgs/MoveToTargetPose.h> 11 #include <choreo_msgs/TaskSequencePlanning.h> 18 traj_lib.
get().clear();
19 for (std::size_t k = 0; k < plan.
plans.size(); ++k)
21 traj_lib.
get()[std::to_string(k)] = plan.
plans[k];
24 return plan.
plans.size() > 0;
31 bool fetch_co_success =
false;
32 std::vector<choreo_msgs::UnitProcessPlan> process_plan;
36 std::replace(saved_graph_file_name.begin(), saved_graph_file_name.end(),
'/',
'_');
39 choreo_msgs::TaskSequencePlanning ts_srv;
40 ts_srv.request.action = ts_srv.request.READ_WIREFRAME;
45 choreo_msgs::ProcessPlanning srv;
46 srv.request.index = selected_path_index;
48 srv.request.file_name = saved_graph_file_name;
49 srv.request.clt_rrt_unit_process_timeout =
model_input_params_.clt_rrt_unit_process_timeout;
54 assert(
as_pnp_.sequenced_elements.size() > 0);
60 fetch_co_success =
true;
70 ROS_ERROR_STREAM(
"[Core] task sequence planning service read wireframe failed.");
71 fetch_co_success =
false;
75 ts_srv.request.action = ts_srv.request.REQUEST_COLLISION_OBJS;
76 ts_srv.request.element_array = std::vector<choreo_msgs::ElementCandidatePoses>(
81 ROS_WARN_STREAM(
"[Core] task sequence planning service construct collision objects failed.");
82 fetch_co_success =
false;
86 srv.request.wf_collision_objs = ts_srv.response.wf_collision_objs;
87 fetch_co_success =
true;
92 srv.request.env_collision_objs =
env_objs_;
95 assert(fetch_co_success);
101 process_plan = srv.response.plan;
103 for (
auto v : process_plan)
105 result.
plans.push_back(
v);
110 ROS_ERROR_STREAM(
"[Core] Failed to plan until path #" << selected_path_index <<
", planning failed.");
119 choreo_msgs::MoveToTargetPose srv;
121 srv.request.type = srv.request.JOINT_POSE;
122 srv.request.pose = joint_pose;
124 for(
auto j : joint_pose)
bool moveToTargetJointPose(std::vector< double > joint_pose)
std::vector< choreo_msgs::UnitProcessPlan > plans
bool generateMotionLibrary(const int selected_path_index, choreo_core_service::TrajectoryLibrary &traj_lib)
ros::ServiceClient move_to_pose_client_
bool call(MReq &req, MRes &res)
ros::ServiceClient process_planning_client_
std::vector< moveit_msgs::CollisionObject > env_objs_
std::string assembly_type_
ros::ServiceClient task_sequence_planning_srv_client_
static const std::string ASSEMBLY_TYPE_PICKNPLACE
choreo_msgs::ModelInputParameters model_input_params_
choreo_msgs::TaskSequenceInputParameters task_sequence_input_params_
#define ROS_WARN_STREAM(args)
#define ROS_INFO_STREAM(args)
static const std::string ASSEMBLY_TYPE_SPATIAL_EXTRUSION
ProcessPlanResult generateProcessPlan(const int &index)
#define ROS_ERROR_STREAM(args)
std::vector< choreo_msgs::ElementCandidatePoses > task_sequence_
choreo_msgs::AssemblySequencePickNPlace as_pnp_