Merge branch 'master' of github.com:ros-industrial-consortium/scan_n_…

…plan_workshop
BryanMqz · May 30, 2024 · 81ae26a · 81ae26a
2 parents 0a3db40 + fccc770
commit 81ae26a
Show file tree

Hide file tree

Showing 30 changed files with 2,540 additions and 288 deletions.
diff --git a/dependencies.repos b/dependencies.repos
@@ -5,15 +5,15 @@
 - git:
     local-name: noether
     uri: https://github.com/ros-industrial/noether.git
-    version: 548182e74897cd3948e97023aad95b3a9d21ce8f
+    version: 537151dd42076e635eb10a053371d5e9dcb4f056
 - git:
     local-name: industrial_reconstruction
     uri: https://github.com/ros-industrial/industrial_reconstruction.git
     version: 6e1eadada1f86b716a6fcef0ae5ffb291f9b5e66
 - git:
     local-name: rviz_polygon_selection_tool
-    uri: https://github.com/marip8/rviz_polygon_selection_tool.git
-    version: c2bff22f4990f04b8b10d64bbbd832618e0b0c02
+    uri: https://github.com/swri-robotics/rviz_polygon_selection_tool.git
+    version: 1.0.0
 - git:
     local-name: BehaviorTree.CPP
     uri: https://github.com/BehaviorTree/BehaviorTree.CPP.git

diff --git a/snp_application/CHANGELOG.rst b/snp_application/CHANGELOG.rst
diff --git a/snp_application/config/snp.btproj b/snp_application/config/snp.btproj
@@ -24,6 +24,12 @@
             <input_port name="action_name" default="follow_joint_trajectory"/>
             <input_port name="trajectory" default="{trajectory}"/>
         </Action>
+        <Action ID="GenerateFreespaceMotionPlanService" editable="true">
+            <input_port name="service_name" default="generate_freespace_motion_plan"/>
+            <input_port name="start_joint_state" default="{start_joint_state}"/>
+            <input_port name="goal_joint_state" default="{goal_joint_state}"/>
+            <output_port name="trajectory" default="{trajectory}"/>
+        </Action>
         <Action ID="GenerateMotionPlanService" editable="true">
             <input_port name="service_name" default="generate_motion_plan"/>
             <input_port name="tool_paths" default="{tool_paths}"/>
@@ -41,6 +47,10 @@
             <input_port name="service_name" default="generate_tool_paths"/>
             <input_port name="tool_paths" default="{tool_paths}"/>
         </Action>
+        <Action ID="GetCurrentJointState" editable="true">
+            <input_port name="topic_name" default="/joint_states"/>
+            <output_port name="current_state" default="{current_state}"/>
+        </Action>
         <Action ID="MotionPlanPub" editable="true">
             <input_port name="topic_name"/>
             <input_port name="trajectory"/>
@@ -73,8 +83,8 @@
             <input_port name="service_name"/>
         </Action>
         <Action ID="UpdateTrajectoryStartState" editable="true">
-            <input_port name="topic_name" default="/joint_states"/>
-            <input_port name="input" default="{trajectory}"/>
+            <input_port name="joint_state" default="{current_state}"/>
+            <input_port name="input_trajectory" default="{trajectory}"/>
             <output_port name="output" default="{trajectory}"/>
         </Action>
         <SubTree ID="motion" editable="true">

diff --git a/snp_application/config/snp.xml b/snp_application/config/snp.xml
@@ -1,5 +1,64 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <root BTCPP_format="4">
+  <BehaviorTree ID="go_home_main">
+    <ForceSuccess>
+      <ReactiveSequence>
+        <RosSpinner/>
+        <ButtonMonitor name="Reset"
+                       button="reset"
+                       _description="Sequence has been reset to start state"/>
+        <KeepRunningUntilFailure>
+          <ForceSuccess>
+            <ReactiveSequence>
+              <ButtonMonitor name="Halt"
+                             button="halt"
+                             _description="Current task has been halted"/>
+              <SubTree ID="go_home_process"
+                       _autoremap="true"/>
+            </ReactiveSequence>
+          </ForceSuccess>
+        </KeepRunningUntilFailure>
+      </ReactiveSequence>
+    </ForceSuccess>
+  </BehaviorTree>
+
+  <BehaviorTree ID="go_home_process">
+    <SequenceWithMemory>
+      <Progress start="0"
+                end="50">
+        <Sequence>
+          <GetCurrentJointState topic_name="/joint_states"
+                                current_state="{current_state}"/>
+          <GenerateFreespaceMotionPlanService service_name="generate_freespace_motion_plan"
+                                              start_joint_state="{current_state}"
+                                              goal_joint_state="{home_state}"
+                                              trajectory="{trajectory}"/>
+          <MotionPlanPub topic_name="motion_plan"
+                         trajectory="{trajectory}"/>
+        </Sequence>
+      </Progress>
+      <Progress start="50"
+                end="100">
+        <Sequence>
+          <SetPage index="3">
+            <ButtonApproval name="Approve Scan Execution"
+                            approve_button="execute"
+                            disapprove_button="back"
+                            _description="Press next to approve the scan motion plan and proceed to scan motion execution"/>
+          </SetPage>
+          <SubTree ID="motion"
+                   trajectory="{trajectory}"
+                   _autoremap="true"/>
+        </Sequence>
+      </Progress>
+      <SetPage index="4">
+        <ButtonApproval name="Approve Restart"
+                        approve_button=""
+                        disapprove_button="back"/>
+      </SetPage>
+    </SequenceWithMemory>
+  </BehaviorTree>
+
   <BehaviorTree ID="main">
     <ForceSuccess>
       <ReactiveSequence>
@@ -26,9 +85,11 @@
     <Sequence>
       <TriggerService name="Enable Robot"
                       service_name="robot_enable"/>
+      <GetCurrentJointState topic_name="/joint_states"
+                            current_state="{current_state}"/>
       <UpdateTrajectoryStartState name="Update Trajectory Start State"
-                                  topic_name="/joint_states"
-                                  input="{trajectory}"
+                                  joint_state="{current_state}"
+                                  input_trajectory="{trajectory}"
                                   output="{updated_trajectory}"/>
       <FollowJointTrajectoryAction name="Execute Approach Motion"
                                    action_name="{follow_joint_trajectory_action}"
@@ -100,29 +161,33 @@
       </Progress>
       <Progress start="60"
                 end="80">
-        <Sequence>
-          <SetPage index="1">
-            <ButtonApproval name="Approve Motion Plan Generation"
-                            approve_button="plan"
-                            disapprove_button="back"
-                            _description="Press next to generate a motion plan for the tool path"/>
-          </SetPage>
-          <GenerateMotionPlanService name="Generate Process Motion Plan"
-                                     service_name="generate_motion_plan"
-                                     tool_paths="{tool_paths}"
-                                     approach="{approach}"
-                                     process="{process}"
-                                     departure="{departure}"/>
-          <CombineTrajectories first="{approach}"
-                               second="{process}"
-                               output="{trajectory}"/>
-          <CombineTrajectories first="{trajectory}"
-                               second="{departure}"
-                               output="{trajectory}"/>
-          <MotionPlanPub name="Publish Process Motion Plan"
-                         topic_name="motion_plan"
-                         trajectory="{trajectory}"/>
-        </Sequence>
+        <ReactiveSequence>
+          <ButtonMonitor name="Tool Path Configuration Changed"
+                         button="tpp_config"/>
+          <Sequence>
+            <SetPage index="1">
+              <ButtonApproval name="Approve Motion Plan Generation"
+                              approve_button="plan"
+                              disapprove_button="back"
+                              _description="Press next to generate a motion plan for the tool path"/>
+            </SetPage>
+            <GenerateMotionPlanService name="Generate Process Motion Plan"
+                                       service_name="generate_motion_plan"
+                                       tool_paths="{tool_paths}"
+                                       approach="{approach}"
+                                       process="{process}"
+                                       departure="{departure}"/>
+            <CombineTrajectories first="{approach}"
+                                 second="{process}"
+                                 output="{trajectory}"/>
+            <CombineTrajectories first="{trajectory}"
+                                 second="{departure}"
+                                 output="{trajectory}"/>
+            <MotionPlanPub name="Publish Process Motion Plan"
+                           topic_name="motion_plan"
+                           trajectory="{trajectory}"/>
+          </Sequence>
+        </ReactiveSequence>
       </Progress>
       <Progress start="80"
                 end="100">
@@ -150,9 +215,11 @@
     <Sequence>
       <TriggerService name="Enable Robot"
                       service_name="robot_enable"/>
+      <GetCurrentJointState topic_name="/joint_states"
+                            current_state="{current_state}"/>
       <UpdateTrajectoryStartState name="Update Trajectory Start State"
-                                  topic_name="/joint_states"
-                                  input="{approach}"
+                                  joint_state="{current_state}"
+                                  input_trajectory="{approach}"
                                   output="{updated_approach}"/>
       <FollowJointTrajectoryAction name="Execute Scan Approach Motion"
                                    action_name="{follow_joint_trajectory_action}"
@@ -196,6 +263,17 @@
       <input_port name="trajectory"
                   default="{trajectory}"/>
     </Action>
+    <Action ID="GenerateFreespaceMotionPlanService"
+            editable="true">
+      <input_port name="service_name"
+                  default="generate_freespace_motion_plan"/>
+      <input_port name="start_joint_state"
+                  default="{start_joint_state}"/>
+      <input_port name="goal_joint_state"
+                  default="{goal_joint_state}"/>
+      <output_port name="trajectory"
+                   default="{trajectory}"/>
+    </Action>
     <Action ID="GenerateMotionPlanService"
             editable="true">
       <input_port name="service_name"
@@ -227,6 +305,13 @@
       <input_port name="tool_paths"
                   default="{tool_paths}"/>
     </Action>
+    <Action ID="GetCurrentJointState"
+            editable="true">
+      <input_port name="topic_name"
+                  default="/joint_states"/>
+      <output_port name="current_state"
+                   default="{current_state}"/>
+    </Action>
     <Action ID="MotionPlanPub"
             editable="true">
       <input_port name="topic_name"/>
@@ -271,9 +356,9 @@
     </Action>
     <Action ID="UpdateTrajectoryStartState"
             editable="true">
-      <input_port name="topic_name"
-                  default="/joint_states"/>
-      <input_port name="input"
+      <input_port name="joint_state"
+                  default="{current_state}"/>
+      <input_port name="input_trajectory"
                   default="{trajectory}"/>
       <output_port name="output"
                    default="{trajectory}"/>

diff --git a/snp_application/include/snp_application/bt/snp_bt_ros_nodes.h b/snp_application/include/snp_application/bt/snp_bt_ros_nodes.h
@@ -13,6 +13,7 @@
 #include <sensor_msgs/msg/joint_state.hpp>
 #include <snp_msgs/srv/execute_motion_plan.hpp>
 #include <snp_msgs/srv/generate_motion_plan.hpp>
+#include <snp_msgs/srv/generate_freespace_motion_plan.hpp>
 #include <snp_msgs/srv/generate_scan_motion_plan.hpp>
 #include <snp_msgs/srv/generate_tool_paths.hpp>
 #include <std_srvs/srv/trigger.hpp>
@@ -26,6 +27,9 @@ static const std::string TCP_FRAME_PARAM = "tcp_frame";
 static const std::string CAMERA_FRAME_PARAM = "camera_frame";
 static const std::string MESH_FILE_PARAM = "mesh_file";
 static const std::string START_STATE_REPLACEMENT_TOLERANCE_PARAM = "start_state_replacement_tolerance";
+// Home state
+static const std::string HOME_STATE_JOINT_VALUES_PARAM = "home_state_joint_values";
+static const std::string HOME_STATE_JOINT_NAMES_PARAM = "home_state_joint_names";
 //   Industrial Reconstruction
 static const std::string IR_TSDF_VOXEL_PARAM = "ir.tsdf.voxel_length";
 static const std::string IR_TSDF_SDF_PARAM = "ir.tsdf.sdf_trunc";
@@ -186,6 +190,25 @@ class GenerateMotionPlanServiceNode : public SnpRosServiceNode<snp_msgs::srv::Ge
   BT::NodeStatus onResponseReceived(const typename Response::SharedPtr& response) override;
 };
 
+class GenerateFreespaceMotionPlanServiceNode : public SnpRosServiceNode<snp_msgs::srv::GenerateFreespaceMotionPlan>
+{
+public:
+  inline static std::string START_JOINT_STATE_INPUT_PORT_KEY = "start_joint_state";
+  inline static std::string GOAL_JOINT_STATE_INPUT_PORT_KEY = "goal_joint_state";
+  inline static std::string TRAJECTORY_OUTPUT_PORT_KEY = "trajectory";
+  inline static BT::PortsList providedPorts()
+  {
+    return providedBasicPorts({ BT::InputPort<sensor_msgs::msg::JointState>(START_JOINT_STATE_INPUT_PORT_KEY),
+                                BT::InputPort<sensor_msgs::msg::JointState>(GOAL_JOINT_STATE_INPUT_PORT_KEY),
+                                BT::OutputPort<trajectory_msgs::msg::JointTrajectory>(TRAJECTORY_OUTPUT_PORT_KEY) });
+  }
+
+  using SnpRosServiceNode<snp_msgs::srv::GenerateFreespaceMotionPlan>::SnpRosServiceNode;
+
+  bool setRequest(typename Request::SharedPtr& request) override;
+  BT::NodeStatus onResponseReceived(const typename Response::SharedPtr& response) override;
+};
+
 class GenerateScanMotionPlanServiceNode : public SnpRosServiceNode<snp_msgs::srv::GenerateScanMotionPlan>
 {
 public:
@@ -281,22 +304,28 @@ class FollowJointTrajectoryActionNode : public SnpRosActionNode<control_msgs::ac
   BT::NodeStatus onResultReceived(const WrappedResult& result) override;
 };
 
-class UpdateTrajectoryStartStateNode : public BT::RosTopicSubNode<sensor_msgs::msg::JointState>
+class UpdateTrajectoryStartStateNode : public BT::ActionNodeBase
 {
 public:
-  inline static std::string TRAJECTORY_INPUT_PORT_KEY = "input";
+  inline static std::string START_JOINT_STATE_INPUT_PORT_KEY = "joint_state";
+  inline static std::string TRAJECTORY_INPUT_PORT_KEY = "input_trajectory";
   inline static std::string TRAJECTORY_OUTPUT_PORT_KEY = "output";
   inline static BT::PortsList providedPorts()
   {
-    return providedBasicPorts({ BT::InputPort<trajectory_msgs::msg::JointTrajectory>(TRAJECTORY_INPUT_PORT_KEY),
-                                BT::OutputPort<trajectory_msgs::msg::JointTrajectory>(TRAJECTORY_OUTPUT_PORT_KEY) });
+    return { BT::InputPort<sensor_msgs::msg::JointState>(START_JOINT_STATE_INPUT_PORT_KEY),
+             BT::InputPort<trajectory_msgs::msg::JointTrajectory>(TRAJECTORY_INPUT_PORT_KEY),
+             BT::OutputPort<trajectory_msgs::msg::JointTrajectory>(TRAJECTORY_OUTPUT_PORT_KEY) };
   }
-  using BT::RosTopicSubNode<sensor_msgs::msg::JointState>::RosTopicSubNode;
+  explicit UpdateTrajectoryStartStateNode(const std::string& instance_name, const BT::NodeConfig& config,
+                                          rclcpp::Node::SharedPtr node);
 
-  BT::NodeStatus onTick(const typename sensor_msgs::msg::JointState::SharedPtr& last_msg) override;
+protected:
+  BT::NodeStatus tick() override;
+  void halt() override;
+  rclcpp::Node::SharedPtr node_;
 };
 
-class ReverseTrajectoryNode : public BT::ControlNode
+class ReverseTrajectoryNode : public BT::ActionNodeBase
 {
 public:
   inline static std::string TRAJECTORY_INPUT_PORT_KEY = "input";
@@ -308,10 +337,12 @@ class ReverseTrajectoryNode : public BT::ControlNode
   }
   explicit ReverseTrajectoryNode(const std::string& instance_name, const BT::NodeConfig& config);
 
+protected:
   BT::NodeStatus tick() override;
+  void halt() override;
 };
 
-class CombineTrajectoriesNode : public BT::ControlNode
+class CombineTrajectoriesNode : public BT::ActionNodeBase
 {
 public:
   inline static std::string FIRST_TRAJECTORY_INPUT_PORT_KEY = "first";
@@ -325,7 +356,22 @@ class CombineTrajectoriesNode : public BT::ControlNode
   }
   explicit CombineTrajectoriesNode(const std::string& instance_name, const BT::NodeConfig& config);
 
+protected:
   BT::NodeStatus tick() override;
+  void halt() override;
+};
+
+class GetCurrentJointStateNode : public BT::RosTopicSubNode<sensor_msgs::msg::JointState>
+{
+public:
+  inline static std::string JOINT_STATE_OUTPUT_PORT_KEY = "current_state";
+  inline static BT::PortsList providedPorts()
+  {
+    return providedBasicPorts({ BT::OutputPort<sensor_msgs::msg::JointState>(JOINT_STATE_OUTPUT_PORT_KEY) });
+  }
+  using BT::RosTopicSubNode<sensor_msgs::msg::JointState>::RosTopicSubNode;
+
+  BT::NodeStatus onTick(const typename sensor_msgs::msg::JointState::SharedPtr& last_msg) override;
 };
 
 /**

diff --git a/snp_application/include/snp_application/snp_widget.h b/snp_application/include/snp_application/snp_widget.h
@@ -23,7 +23,7 @@ class SNPWidget : public QWidget
   explicit SNPWidget(rclcpp::Node::SharedPtr rviz_node, QWidget* parent = nullptr);
 
 protected:
-  void runTreeWithThread();
+  void runTreeWithThread(const std::string& bt_tree_name);
 
   virtual BT::BehaviorTreeFactory createBTFactory(int ros_short_timeout, int ros_long_timeout);
   QStackedWidget* getStackedWidget();

diff --git a/snp_application/package.xml b/snp_application/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>snp_application</name>
-  <version>0.0.0</version>
+  <version>4.6.1</version>
   <description>
     GUI application to run Scan-N-Plan for ROSCon 2021 workshop.
   </description>