nic/drawing-robot-ros2
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fix runtime issues

Browse Source
This commit is contained in:
Nicolas Hiillos
2023-03-01 10:51:54 +02:00
parent dd7893e207
commit de57ae288d
3 changed files with 41 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
src/lite6_controller/launch/lite6_real.launch.py
View File

@@ -231,7 +231,8 @@ def launch_setup(context, *args, **kwargs):
pilz_planning_pipeline_config = { pilz_planning_pipeline_config = {
'move_group': { 'move_group': {
'planning_plugin': 'pilz_industrial_motion_planner/CommandPlanner', 'planning_plugin': 'pilz_industrial_motion_planner/CommandPlanner',
'request_adapters': """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""", #'request_adapters': """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
'request_adapters': """default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
"default_planner_config": "PTP", "default_planner_config": "PTP",
} }
} }

3
src/lite6_controller/launch/lite6_real_no_gui.launch.py
View File

@@ -196,7 +196,8 @@ def launch_setup(context, *args, **kwargs):
pilz_planning_pipeline_config = { pilz_planning_pipeline_config = {
'move_group': { 'move_group': {
'planning_plugin': 'pilz_industrial_motion_planner/CommandPlanner', 'planning_plugin': 'pilz_industrial_motion_planner/CommandPlanner',
'request_adapters': """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""", #'request_adapters': """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
'request_adapters': """default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
"default_planner_config": "PTP", "default_planner_config": "PTP",
} }
} }

53
src/lite6_controller/src/lite6_controller.cpp
View File

@@ -75,7 +75,7 @@ public:
// Set limits for A4 paper // Set limits for A4 paper
// 297x210 // 297x210
float xlim_lower = 0.10; float xlim_lower = 0.20;
float xlim_upper = 0.25; float xlim_upper = 0.25;
float ylim_lower = -0.10; float ylim_lower = -0.10;
float ylim_upper = 0.115; float ylim_upper = 0.115;
@@ -375,17 +375,19 @@ public:
long n = 0; long n = 0;
for (auto p : goal->motion.path) for (auto p : goal->motion.path)
{ {
// skip first
//if (n <= 0) continue; //if (n <= 0) continue;
//else n--; //n--;
//RCLCPP_INFO(this->get_logger(), "Creating MSI"); //RCLCPP_INFO(this->get_logger(), "Creating MSI");
moveit_msgs::msg::MotionSequenceItem msi = moveit_msgs::msg::MotionSequenceItem(); moveit_msgs::msg::MotionSequenceItem msi = moveit_msgs::msg::MotionSequenceItem();
planning_interface::MotionPlanRequest mpr = planning_interface::MotionPlanRequest(); planning_interface::MotionPlanRequest mpr = planning_interface::MotionPlanRequest();
mpr.planner_id = "PTP"; mpr.planner_id = "PTP";
mpr.group_name = move_group.getName(); mpr.group_name = move_group.getName();
mpr.max_velocity_scaling_factor = 1.0; mpr.max_velocity_scaling_factor = 0.5;
mpr.max_acceleration_scaling_factor = 1.0; mpr.max_acceleration_scaling_factor = 0.5;
mpr.allowed_planning_time = 100; mpr.allowed_planning_time = 10;
mpr.max_cartesian_speed = 1; // m/s mpr.max_cartesian_speed = 1; // m/s
//mpr.goal_constraints.position_constraints.header.frame_id = "world"; //mpr.goal_constraints.position_constraints.header.frame_id = "world";
@@ -403,15 +405,20 @@ public:
//moveit_msgs::msg::Constraints pose_goal = //moveit_msgs::msg::Constraints pose_goal =
// kinematic_constraints::constructGoalConstraints(ee_link, p, tolerance_pose, tolerance_angle); // kinematic_constraints::constructGoalConstraints(ee_link, p, tolerance_pose, tolerance_angle);
//kinematic_constraints::constructGoalConstraints(ee_link, pose, tolerance_pose, tolerance_angle); //kinematic_constraints::constructGoalConstraints(ee_link, pose, tolerance_pose, tolerance_angle);
geometry_msgs::msg::PointStamped point;
auto position = translatePose(p).pose.position;
point.point = position;
moveit_msgs::msg::Constraints pose_goal = moveit_msgs::msg::Constraints pose_goal =
kinematic_constraints::constructGoalConstraints(ee_link, translatePose(p), 1e-6, 1e-6); //kinematic_constraints::constructGoalConstraints(ee_link, point, 1e-5);
kinematic_constraints::constructGoalConstraints(ee_link, translatePose(p), 1e-5, 1e-2);
//kinematic_constraints::constructGoalConstraints(ee_link, p, 1.0, 1.0); //kinematic_constraints::constructGoalConstraints(ee_link, p, 1.0, 1.0);
//kinematic_constraints::constructGoalConstraints(ee_link, p, 1e-3, 1e-2); //kinematic_constraints::constructGoalConstraints(ee_link, p, 1e-3, 1e-2);
mpr.goal_constraints.push_back(pose_goal); mpr.goal_constraints.push_back(pose_goal);
msi.req = mpr; msi.req = mpr;
msi.blend_radius = 0.0001; //TODO make configurable msi.blend_radius = 0.00000001; //TODO make configurable
msr.items.push_back(msi); msr.items.push_back(msi);
} }
msr.items.back().blend_radius = 0.0; // Last element blend must be 0 msr.items.back().blend_radius = 0.0; // Last element blend must be 0
@@ -433,21 +440,35 @@ public:
auto res = sequence_client_->async_send_request(req); auto res = sequence_client_->async_send_request(req);
// Wait for the result. // Wait for the result.
RCLCPP_INFO(this->get_logger(), "Waiting for result"); RCLCPP_INFO(this->get_logger(), "Waiting for result");
res.wait(); auto status = res.wait_for(30s);
try if (status == std::future_status::ready)
{ {
for (auto t : res.get()->response.planned_trajectories) auto ts = res.get()->response.planned_trajectories;
RCLCPP_INFO(this->get_logger(), "Got %ld trajectories", ts.size());
if (ts.size() > 0)
{ {
// TODO RCLCPP_INFO(this->get_logger(), "Executing result");
//trajectory->append(t, 0.0001, 0); move_group.execute(ts[0]);
RCLCPP_INFO(this->get_logger(), "Executing trajectory of length: %ld", t.joint_trajectory.points.size());
this->move_group.execute(t);
} }
} }
catch(const std::exception& e) else
{ {
RCLCPP_ERROR(this->get_logger(), "Failed to call motion sequence service"); RCLCPP_ERROR(this->get_logger(), "Planner failed to return trajectory in time");
} }
//for (auto t : res.get()->response.planned_trajectories)
//{
// // TODO
// //trajectory->append(t, 0.0001, 0);
// RCLCPP_INFO(this->get_logger(), "Executing trajectory of length: %ld", t.joint_trajectory.points.size());
// this->move_group.execute(t);
//}
//try
//{
//}
//catch(const std::exception& e)
//{
// RCLCPP_ERROR(this->get_logger(), "Failed to call motion sequence service");
//}
//if (plan_success) //if (plan_success)
//{ //{
// //trajectory.setRobotTrajectoryMsg(*move_group_state, trajectory_msg); // //trajectory.setRobotTrajectoryMsg(*move_group_state, trajectory_msg);