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

Expose ros2 params and load config yaml

Browse Source
This commit is contained in:
Nicolas Hiillos
2023-04-11 11:31:38 +03:00
parent 925a9b42c7
commit 7f547c65f7
6 changed files with 47 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
src/lite6_controller/CMakeLists.txt
View File

@@ -72,4 +72,9 @@ install(TARGETS
install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
install(DIRECTORY
config
DESTINATION share/${PROJECT_NAME}
)
ament_package()

4
src/lite6_controller/launch/lite6_gazebo.launch.py
View File

@@ -19,6 +19,7 @@ def launch_setup(context, *args, **kwargs):
log_level = LaunchConfiguration("log_level", default='info')
rviz_config = LaunchConfiguration('rviz_config', default=os.path.join(get_package_share_directory("custom_xarm_description"), "rviz", "display.rviz"))
prefix = LaunchConfiguration('prefix', default='')
hw_ns = LaunchConfiguration('hw_ns', default='xarm')
limited = LaunchConfiguration('limited', default=False)
@@ -189,6 +190,7 @@ def launch_setup(context, *args, **kwargs):
robot_description,
robot_description_semantic,
{"use_sim_time": use_sim_time},
lite6_config,
],
),
Node(
@@ -263,6 +265,8 @@ def launch_setup(context, *args, **kwargs):
kinematics_yaml = robot_description_parameters['robot_description_kinematics']
joint_limits_yaml = robot_description_parameters.get('robot_description_planning', None)
lite6_config = load_yaml('lite6_controller', 'config', 'config.yaml')
#cartesian_limits = load_yaml(moveit_config_package_name, 'config', xarm_type, 'cartesian_limits.yaml')
#robot_description_parameters['robot_description_planning']['cartesian_limits'] = cartesian_limits['cartesian_limits']
#input(joint_limits_yaml)

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

@@ -15,6 +15,7 @@ from launch.events import Shutdown
def launch_setup(context, *args, **kwargs):
robot_ip = LaunchConfiguration('robot_ip', default='192.168.1.150')
report_type = LaunchConfiguration('report_type', default='normal')
prefix = LaunchConfiguration('prefix', default='')
@@ -164,6 +165,8 @@ 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')
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)))
gripper_ompl_planning_yaml = load_yaml(moveit_config_package_name, 'config', '{}_gripper'.format(robot_type.perform(context)), 'ompl_planning.yaml')
@@ -352,6 +355,7 @@ def launch_setup(context, *args, **kwargs):
#robot_description_parameters['robot_description_kinematics'],
robot_description_parameters,
{"use_sim_time": use_sim_time},
lite6_config,
],
),
]

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

@@ -15,6 +15,7 @@ from launch.events import Shutdown
def launch_setup(context, *args, **kwargs):
robot_ip = LaunchConfiguration('robot_ip', default='192.168.1.150')
report_type = LaunchConfiguration('report_type', default='normal')
prefix = LaunchConfiguration('prefix', default='')
@@ -155,6 +156,9 @@ def launch_setup(context, *args, **kwargs):
kinematics_yaml = robot_description_parameters['robot_description_kinematics']
joint_limits_yaml = robot_description_parameters.get('robot_description_planning', None)
#lite6_config = load_yaml('lite6_controller', 'config', 'config.yaml')
lite6_config = os.path.join('lite6_controller', 'config', 'config.yaml')
# FIX acceleration limits
#for i in range(1,7):
# joint_limits_yaml['joint_limits']['joint{}'.format(i)]['has_acceleration_limits'] = True
@@ -342,6 +346,7 @@ def launch_setup(context, *args, **kwargs):
#robot_description_parameters['robot_description_kinematics'],
robot_description_parameters,
{"use_sim_time": use_sim_time},
lite6_config,
],
),
]

31
src/lite6_controller/src/lite6_controller.cpp
View File

@@ -80,8 +80,7 @@ public:
float xlim_upper = 0.305;
float ylim_lower = -0.1475;
float ylim_upper = 0.1475;
//float zlim_lower = 0.1945;
float zlim_lower = 0.207493;
float zlim_lower = 0.141087;
float zlim_upper = zlim_lower + 0.01;
@@ -92,6 +91,24 @@ public:
: RobotController(options),
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);
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();
this->move_group.setMaxAccelerationScalingFactor(1.0);
this->move_group.setMaxVelocityScalingFactor(1.0);
//this->move_group.setMaxVelocityScalingFactor(0.8);
@@ -199,8 +216,9 @@ public:
//mpr.planner_id = "LIN";
mpr.group_name = move_group.getName();
//mpr.max_velocity_scaling_factor = 1.0;
mpr.max_velocity_scaling_factor = 1.0;
mpr.max_acceleration_scaling_factor = 0.8;
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_acceleration_scaling_factor = 0.1;
mpr.allowed_planning_time = 20;
//mpr.max_cartesian_speed = 2; // m/s
@@ -232,9 +250,7 @@ public:
mpr.goal_constraints.push_back(pose_goal);
msi.req = mpr;
//msi.blend_radius = 6e-7; //TODO make configurable
//msi.blend_radius = 0.000001; //TODO make configurable
msi.blend_radius = 0.0; //TODO make configurable
msi.blend_radius = this->get_parameter("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");
@@ -250,6 +266,7 @@ public:
msr.items.push_back(msi);
}
msr.items.back().blend_radius = 0.0; // Last element blend must be 0
moveit_msgs::msg::RobotState state_msg;
if (move_group_state == NULL)
{