robossembler/runtime
7
0
Fork 0
Code Issues 5 Pull requests Projects Releases Packages Wiki Activity Actions

enhance skill server parameter handling and assembly service

Browse source 
- Unified `base_link` and `ee_link` parameter usage across launch files and skill server components.
- Improved synchronization in `mtjs_jtc` and `mtp_jtc` with a timeout mechanism for joint position updates, adding error handling when data is unavailable.
- Extended `AssemblyConfigService` to broadcast TF transforms for relative parts during initialization.
- Enhanced YAML parsing for `AssemblyConfigService` to handle missing orientations with default values.
- Updated `mtp_jtc_cart` to align parameter names with other skill server components.
- Added single-threaded executor to `AssemblyConfigService` for better lifecycle management.
This commit is contained in:
Ilya Uraev 2024-11-30 19:32:37 +03:00
parent de0d5d4440
commit c4c13d55b1
5 changed files with 127 additions and 48 deletions
Download patch file Download diff file Expand all files Collapse all files

5
rbs_skill_servers/launch/skills.launch.py
View file

@ -55,6 +55,8 @@ def launch_setup(context, *args, **kwargs):
parameters=[
{"use_sim_time": use_sim_time},
{"robot_name": namespace},
{"base_link": base_link_name},
{"ee_link": ee_link_name},
robot_description,
],
),
@ -66,7 +68,7 @@ def launch_setup(context, *args, **kwargs):
{"use_sim_time": use_sim_time},
{"robot_name": namespace},
{"base_link": base_link_name},
{"robot_ee_link": ee_link_name},
{"ee_link": ee_link_name},
robot_description,
],
),
@ -101,6 +103,7 @@ def launch_setup(context, *args, **kwargs):
package="rbs_utils",
executable="assembly_config_service.py",
namespace=namespace,
parameters=[{"use_sim_time": use_sim_time}],
output="screen",
)

12
rbs_skill_servers/src/mtjs_jtc.cpp
View file

@ -60,8 +60,16 @@ protected:
trajectory_goal.trajectory.joint_names = m_joint_names;
// TODO: Better sync solution
while (m_current_joint_positions.empty()) {
const int max_wait_iterations = 100;
int wait_count = 0;
while (m_current_joint_positions.empty() &&
wait_count++ < max_wait_iterations) {
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
if (m_current_joint_positions.empty()) {
RCLCPP_ERROR(this->get_logger(),
"Joint positions were not received in time");
return;
}
trajectory_goal.trajectory.points = generate_trajectory(

36
rbs_skill_servers/src/mtp_jtc.cpp
View file

@ -57,6 +57,12 @@ public:
std::placeholders::_1),
s_options);
this->declare_parameter("robot_description", "");
this->declare_parameter("base_link", "");
this->declare_parameter("ee_link", "");
m_robot_description = this->get_parameter("robot_description").as_string();
m_base_link = this->get_parameter("base_link").as_string();
m_ee_link = this->get_parameter("ee_link").as_string();
}
protected:
@ -75,8 +81,16 @@ protected:
trajectory_goal.trajectory.joint_names = m_joint_names;
// TODO: Better sync solution
while (m_current_joint_positions.empty()) {
const int max_wait_iterations = 100;
int wait_count = 0;
while (m_current_joint_positions.empty() &&
wait_count++ < max_wait_iterations) {
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
if (m_current_joint_positions.empty()) {
RCLCPP_ERROR(this->get_logger(),
"Joint positions were not received in time");
return;
}
std::vector<double> target_joint_values;
@ -86,6 +100,7 @@ protected:
generate_trajectory(m_current_joint_positions, target_joint_values,
m_current_goal->duration);
// RCLCPP_INFO(this->get_logger(), "Starting executing action goal");
auto send_goal_options =
rclcpp_action::Client<FollowJointTrajectory>::SendGoalOptions();
send_goal_options.result_callback =
@ -109,7 +124,7 @@ private:
if (m_joint_names.empty()) {
return;
}
RCLCPP_WARN(this->get_logger(), "Called joint positions");
// RCLCPP_WARN(this->get_logger(), "Called joint positions");
if (m_joint_mame_to_index.empty()) {
m_joint_mame_to_index.reserve(m_joint_names.size());
@ -140,10 +155,12 @@ private:
const std::vector<double> &target_joint_values,
const double duration) {
RCLCPP_INFO(this->get_logger(), "Starting generate_trajectory");
const int num_points = 100;
std::vector<trajectory_msgs::msg::JointTrajectoryPoint> points;
for (int i = 0; i <= num_points; ++i) {
trajectory_msgs::msg::JointTrajectoryPoint point;
double t = static_cast<double>(i) / num_points;
for (size_t j = 0; j < target_joint_values.size(); ++j) {
double position = start_joint_values[j] +
@ -157,6 +174,8 @@ private:
}
void solveIK(std::vector<double> &out) {
RCLCPP_INFO(this->get_logger(), "Starting solveIK");
KDL::JntArray q_in(m_joint_names.size());
for (size_t i = 0; i < m_joint_names.size(); ++i) {
q_in(i) = m_current_joint_positions[i];
@ -175,14 +194,8 @@ private:
target_pose.translation().y(),
target_pose.translation().z()));
const std::string m_base_link = "base_link";
const std::string m_ee_link = "gripper_grasp_point";
auto robot_description =
this->get_parameter("robot_description").as_string();
KDL::Tree kdl_tree;
if (!kdl_parser::treeFromString(robot_description, kdl_tree)) {
if (!kdl_parser::treeFromString(m_robot_description, kdl_tree)) {
RCLCPP_ERROR(this->get_logger(),
"Failed to parse KDL tree from robot description.");
return;
@ -216,6 +229,9 @@ private:
std::vector<double> m_current_joint_positions;
std::unordered_map<std::string, size_t> m_joint_mame_to_index;
std::vector<std::string> m_joint_names;
std::string m_base_link;
std::string m_ee_link;
std::string m_robot_description;
};
} // namespace rbs_skill_actions

4
rbs_skill_servers/src/mtp_jtc_cart.cpp
View file

@ -60,7 +60,7 @@ public:
s_options);
this->declare_parameter("robot_description", "");
this->declare_parameter("base_link", "");
this->declare_parameter("robot_ee_link", "");
this->declare_parameter("ee_link", "");
auto robot_description =
this->get_parameter("robot_description").as_string();
@ -73,7 +73,7 @@ public:
}
auto base_link = this->get_parameter("base_link").as_string();
auto robot_ee_link = this->get_parameter("robot_ee_link").as_string();
auto robot_ee_link = this->get_parameter("ee_link").as_string();
if (base_link.empty() or robot_ee_link.empty()) {
RCLCPP_ERROR(this->get_logger(),
"Describe robot end-effector link and base link to continue");