add fjt action param to snp_application (ros-industrial-consortium#99)

* add fjt action param to snp_application * update snp_widget for clang formatting
Yolnan · Mar 13, 2024 · f0aa0a9 · f0aa0a9
1 parent a91c7e2
commit f0aa0a9
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 6 deletions.
diff --git a/snp_application/config/snp.xml b/snp_application/config/snp.xml
@@ -31,13 +31,13 @@
                                   input="{approach}"
                                   output="{updated_approach}"/>
       <FollowJointTrajectoryAction name="Execute Approach Motion"
-                                   action_name="joint_trajectory_position_controller/follow_joint_trajectory"
+                                   action_name="{follow_joint_trajectory_action}"
                                    trajectory="{updated_approach}"/>
       <FollowJointTrajectoryAction name="Execute Process Motion"
-                                   action_name="joint_trajectory_position_controller/follow_joint_trajectory"
+                                   action_name="{follow_joint_trajectory_action}"
                                    trajectory="{process}"/>
       <FollowJointTrajectoryAction name="Execute Departure Motion"
-                                   action_name="joint_trajectory_position_controller/follow_joint_trajectory"
+                                   action_name="{follow_joint_trajectory_action}"
                                    trajectory="{departure}"/>
       <TriggerService name="Disable Robot"
                       service_name="robot_disable"/>
@@ -156,17 +156,17 @@
                                   input="{approach}"
                                   output="{updated_approach}"/>
       <FollowJointTrajectoryAction name="Execute Scan Approach Motion"
-                                   action_name="joint_trajectory_position_controller/follow_joint_trajectory"
+                                   action_name="{follow_joint_trajectory_action}"
                                    trajectory="{updated_approach}"/>
       <StartReconstructionService name="Start Reconstruction"
                                   service_name="start_reconstruction"/>
       <FollowJointTrajectoryAction name="Execute Scan Motion"
-                                   action_name="joint_trajectory_position_controller/follow_joint_trajectory"
+                                   action_name="{follow_joint_trajectory_action}"
                                    trajectory="{process}"/>
       <StopReconstructionService name="Stop Reconstruction"
                                  service_name="stop_reconstruction"/>
       <FollowJointTrajectoryAction name="Execute Scan Departure Motion"
-                                   action_name="joint_trajectory_position_controller/follow_joint_trajectory"
+                                   action_name="{follow_joint_trajectory_action}"
                                    trajectory="{departure}"/>
       <TriggerService name="Disable Robot"
                       service_name="robot_disable"/>

diff --git a/snp_application/src/snp_widget.cpp b/snp_application/src/snp_widget.cpp
@@ -23,6 +23,7 @@ static const std::string BT_FILES_PARAM = "bt_files";
 static const std::string BT_PARAM = "tree";
 static const std::string BT_SHORT_TIMEOUT_PARAM = "bt_short_timeout";
 static const std::string BT_LONG_TIMEOUT_PARAM = "bt_long_timeout";
+static const std::string FOLLOW_JOINT_TRAJECTORY_ACTION = "follow_joint_trajectory_action";
 
 class TPPDialog : public QDialog
 {
@@ -110,6 +111,7 @@ SNPWidget::SNPWidget(rclcpp::Node::SharedPtr rviz_node, QWidget* parent)
   bt_node_->declare_parameter<std::string>(BT_PARAM, "");
   bt_node_->declare_parameter<int>(BT_SHORT_TIMEOUT_PARAM, 5);    // seconds
   bt_node_->declare_parameter<int>(BT_LONG_TIMEOUT_PARAM, 6000);  // seconds
+  bt_node_->declare_parameter<std::string>(FOLLOW_JOINT_TRAJECTORY_ACTION, "follow_joint_trajectory");
 
   bt_node_->declare_parameter<float>(IR_TSDF_VOXEL_PARAM, 0.01f);
   bt_node_->declare_parameter<float>(IR_TSDF_SDF_PARAM, 0.03f);
@@ -162,6 +164,10 @@ BT::BehaviorTreeFactory SNPWidget::createBTFactory(int ros_short_timeout, int ro
   ros_params.wait_for_server_timeout = std::chrono::seconds(0);
   ros_params.server_timeout = std::chrono::seconds(ros_short_timeout);
 
+  // Get joint trajectory action topic name from parameter and store it in the blackboard
+  board_->set(FOLLOW_JOINT_TRAJECTORY_ACTION,
+              snp_application::get_parameter<std::string>(bt_node_, FOLLOW_JOINT_TRAJECTORY_ACTION));
+
   // Publishers/Subscribers
   bt_factory.registerNodeType<ToolPathsPubNode>("ToolPathsPub", ros_params);
   bt_factory.registerNodeType<MotionPlanPubNode>("MotionPlanPub", ros_params);