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

Expose working lite6 config

Browse Source
This commit is contained in:
Nicolas Hiillos
2023-04-28 11:16:41 +03:00
parent 7f547c65f7
commit cdef55b647
3 changed files with 39 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,4 +1,5 @@
import os
import yaml
from launch import LaunchDescription
from launch.actions import OpaqueFunction, IncludeLaunchDescription, DeclareLaunchArgument
from launch.launch_description_sources import PythonLaunchDescriptionSource
@@ -56,6 +57,8 @@ def launch_setup(context, *args, **kwargs):
use_sim_time = LaunchConfiguration('use_sim_time', default=False)
log_level = LaunchConfiguration("log_level", default='warn')
config = LaunchConfiguration("config", default=PathJoinSubstitution([FindPackageShare('lite6_controller'), 'config', 'config.yaml']))
# # robot driver launch
# # xarm_api/launch/_robot_driver.launch.py
# robot_driver_launch = IncludeLaunchDescription(
@@ -165,7 +168,11 @@ def launch_setup(context, *args, **kwargs):
joint_limits_yaml = robot_description_parameters.get('robot_description_planning', None)
lite6_config = load_yaml('lite6_controller', 'config', 'config.yaml')
robotcontroller_config = config.perform(context)
with open(robotcontroller_config, 'r') as f:
lite6_config = yaml.safe_load(f)
print(f'Loaded configuration: {lite6_config}')
robot_description_parameters.update(lite6_config['/robot_controller']['ros__parameters'])
if add_gripper.perform(context) in ('True', 'true'):
gripper_controllers_yaml = load_yaml(moveit_config_package_name, 'config', '{}_gripper'.format(robot_type.perform(context)), '{}.yaml'.format(controllers_name.perform(context)))
@@ -355,7 +362,6 @@ def launch_setup(context, *args, **kwargs):
#robot_description_parameters['robot_description_kinematics'],
robot_description_parameters,
{"use_sim_time": use_sim_time},
lite6_config,
],
),
]

36
src/lite6_controller/src/lite6_controller.cpp
View File

@@ -92,22 +92,22 @@ public:
move_group(std::shared_ptr<rclcpp::Node>(std::move(this)), MOVE_GROUP)
{
// Declare parameters
this->declare_parameter("x_limit_lower", 0.1);
this->declare_parameter("x_limit_upper", 0.305);
this->declare_parameter("y_limit_lower", -0.1475);
this->declare_parameter("y_limit_upper", 0.1475);
this->declare_parameter("z_offset", 0.141087);
this->declare_parameter("z_lift_amount", 0.01);
this->declare_parameter("max_velocity_scaling_factor", 1.0);
this->declare_parameter("max_acceleration_scaling_factor", 0.6);
this->declare_parameter("blend_radius", 0.0);
this->declare_parameter("lite6_x_limit_lower", 0.1);
this->declare_parameter("lite6_x_limit_upper", 0.305);
this->declare_parameter("lite6_y_limit_lower", -0.1475);
this->declare_parameter("lite6_y_limit_upper", 0.1475);
this->declare_parameter("lite6_z_offset", 0.141087);
this->declare_parameter("lite6_z_lift_amount", 0.01);
this->declare_parameter("lite6_max_velocity_scaling_factor", 1.0);
this->declare_parameter("lite6_max_acceleration_scaling_factor", 0.6);
this->declare_parameter("lite6_blend_radius", 0.0);
xlim_lower = this->get_parameter("x_limit_lower").as_double();
xlim_upper = this->get_parameter("x_limit_upper").as_double();
ylim_lower = this->get_parameter("y_limit_lower").as_double();
ylim_upper = this->get_parameter("y_limit_upper").as_double();
zlim_lower = this->get_parameter("z_offset").as_double();
zlim_upper = zlim_lower + this->get_parameter("z_lift_amount").as_double();
xlim_lower = this->get_parameter("lite6_x_limit_lower").as_double();
xlim_upper = this->get_parameter("lite6_x_limit_upper").as_double();
ylim_lower = this->get_parameter("lite6_y_limit_lower").as_double();
ylim_upper = this->get_parameter("lite6_y_limit_upper").as_double();
zlim_lower = this->get_parameter("lite6_z_offset").as_double();
zlim_upper = zlim_lower + this->get_parameter("lite6_z_lift_amount").as_double();
this->move_group.setMaxAccelerationScalingFactor(1.0);
this->move_group.setMaxVelocityScalingFactor(1.0);
@@ -216,8 +216,8 @@ public:
//mpr.planner_id = "LIN";
mpr.group_name = move_group.getName();
//mpr.max_velocity_scaling_factor = 1.0;
mpr.max_velocity_scaling_factor = this->get_parameter("max_velocity_scaling_factor").as_double();
mpr.max_acceleration_scaling_factor = this->get_parameter("max_acceleration_scaling_factor").as_double();
mpr.max_velocity_scaling_factor = this->get_parameter("lite6_max_velocity_scaling_factor").as_double();
mpr.max_acceleration_scaling_factor = this->get_parameter("lite6_max_acceleration_scaling_factor").as_double();
//mpr.max_acceleration_scaling_factor = 0.1;
mpr.allowed_planning_time = 20;
@@ -250,7 +250,7 @@ public:
mpr.goal_constraints.push_back(pose_goal);
msi.req = mpr;
msi.blend_radius = this->get_parameter("blend_radius").as_double();
msi.blend_radius = this->get_parameter("lite6_blend_radius").as_double();
if (coincidentPoints(&pose.pose.position, &previous_point, msi.blend_radius + 1e-5))
{
RCLCPP_ERROR(this->get_logger(), "Detected coincident points, setting blend radius to 0.0");