diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/CMakeLists.txt b/smacc2_sm_reference_library/sm_multi_panda_sim/CMakeLists.txt new file mode 100644 index 000000000..c445998b3 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/CMakeLists.txt @@ -0,0 +1,71 @@ +cmake_minimum_required(VERSION 3.5) +project(sm_multi_panda_sim) + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(ros_timer_client REQUIRED) +find_package(smacc2 REQUIRED) +find_package(Boost COMPONENTS thread REQUIRED) +find_package(move_group_interface_client REQUIRED) + +find_package(sr_all_events_go REQUIRED) +find_package(ros_publisher_client REQUIRED) +find_package(keyboard_client REQUIRED) + +set(THIS_PACKAGE_INCLUDE_DEPENDS + ros_timer_client + smacc2 + move_group_interface_client + sr_all_events_go + ros_publisher_client + keyboard_client +) + +include_directories( + include + ${smacc2_INCLUDE_DIRS} + ${move_group_interface_client_INCLUDE_DIRS} +) + +add_executable(${PROJECT_NAME}_node + src/${PROJECT_NAME}/${PROJECT_NAME}_node.cpp +) + +target_link_libraries(${PROJECT_NAME}_node + ${smacc2_LIBRARIES} + ${ros_timer_client_LIBRARIES} + ${Boost_LIBRARIES} + ${move_group_interface_client_LIBRARIES} +) + +ament_target_dependencies(${PROJECT_NAME}_node ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +install( + DIRECTORY include/ + DESTINATION include +) + +install(DIRECTORY + config + launch + rviz + # urdf + # srdf + DESTINATION share/${PROJECT_NAME} +) + +install(TARGETS + ${PROJECT_NAME}_node + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/README.md b/smacc2_sm_reference_library/sm_multi_panda_sim/README.md new file mode 100644 index 000000000..e631081ab --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/README.md @@ -0,0 +1,42 @@ +

Build Instructions

+ +First, source your chosen ros2 distro. +``` +source /opt/ros/rolling/setup.bash +``` +``` +source /opt/ros/galactic/setup.bash +``` + +Before you build, make sure you've installed all the dependencies... + +``` +rosdep install --ignore-src --from-paths src -y -r +``` + +Then build with colcon build... + +``` +colcon build +``` +

Operating Instructions

+After you build, remember to source the proper install folder... + +``` +source ~//install/setup.bash +``` + +And then run the launch file... + +``` +ros2 launch sm_multi_panda_sim sm_multi_panda_sim.launch +``` + +

Viewer Instructions

+If you have the SMACC2 Runtime Analyzer installed then type... + +``` +ros2 run smacc2_rta smacc2_rta +``` + +If you don't have the SMACC2 Runtime Analyzer click here diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/config/cartesian_limits.yaml b/smacc2_sm_reference_library/sm_multi_panda_sim/config/cartesian_limits.yaml new file mode 100644 index 000000000..c1e8d321d --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/config/cartesian_limits.yaml @@ -0,0 +1,5 @@ +cartesian_limits: + max_trans_vel: 1.0 + max_trans_acc: 2.25 + max_trans_dec: -5.0 + max_rot_vel: 1.57 diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/config/hand.xacro b/smacc2_sm_reference_library/sm_multi_panda_sim/config/hand.xacro new file mode 100644 index 000000000..566f1c1b6 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/config/hand.xacro @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/config/joint_limits.yaml b/smacc2_sm_reference_library/sm_multi_panda_sim/config/joint_limits.yaml new file mode 100644 index 000000000..ab296964e --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/config/joint_limits.yaml @@ -0,0 +1,55 @@ +# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed +# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] +# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] + +# As MoveIt! does not support jerk limits, the acceleration limits provided here are the highest values that guarantee +# that no jerk limits will be violated. More precisely, applying Euler differentiation in the worst case (from min accel +# to max accel in 1 ms) the acceleration limits are the ones that satisfy +# max_jerk = (max_acceleration - min_acceleration) / 0.001 + +joint_limits: + panda_joint1: + has_velocity_limits: true + max_velocity: 2.1750 + has_acceleration_limits: true + max_acceleration: 3.75 + panda_joint2: + has_velocity_limits: true + max_velocity: 2.1750 + has_acceleration_limits: true + max_acceleration: 1.875 + panda_joint3: + has_velocity_limits: true + max_velocity: 2.1750 + has_acceleration_limits: true + max_acceleration: 2.5 + panda_joint4: + has_velocity_limits: true + max_velocity: 2.1750 + has_acceleration_limits: true + max_acceleration: 3.125 + panda_joint5: + has_velocity_limits: true + max_velocity: 2.6100 + has_acceleration_limits: true + max_acceleration: 3.75 + panda_joint6: + has_velocity_limits: true + max_velocity: 2.6100 + has_acceleration_limits: true + max_acceleration: 5.0 + panda_joint7: + has_velocity_limits: true + max_velocity: 2.6100 + has_acceleration_limits: true + max_acceleration: 5.0 + panda_finger_joint1: + has_velocity_limits: true + max_velocity: 0.1 + has_acceleration_limits: false + max_acceleration: 0.0 + panda_finger_joint2: + has_velocity_limits: true + max_velocity: 0.1 + has_acceleration_limits: false + max_acceleration: 0.0 diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/config/kinematics.yaml b/smacc2_sm_reference_library/sm_multi_panda_sim/config/kinematics.yaml new file mode 100644 index 000000000..36a8b6d76 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/config/kinematics.yaml @@ -0,0 +1,8 @@ +left_panda_arm: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 +# kinematics_solver_timeout: 0.05 +right_panda_arm: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 +# kinematics_solver_timeout: 0.05 diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/config/left_initial_positions.yaml b/smacc2_sm_reference_library/sm_multi_panda_sim/config/left_initial_positions.yaml new file mode 100644 index 000000000..b81bb6029 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/config/left_initial_positions.yaml @@ -0,0 +1,9 @@ +# Default initial positions for the panda arm's ros2_control fake system +initial_positions: + panda_joint1: 0.0 + panda_joint2: -0.785 + panda_joint3: 0.0 + panda_joint4: -2.356 + panda_joint5: 0.0 + panda_joint6: 1.571 + panda_joint7: 0.785 diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/config/moveit_controllers.yaml b/smacc2_sm_reference_library/sm_multi_panda_sim/config/moveit_controllers.yaml new file mode 100644 index 000000000..6942732c2 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/config/moveit_controllers.yaml @@ -0,0 +1,7 @@ +# MoveIt uses this configuration for controller management +trajectory_execution: + allowed_execution_duration_scaling: 1.2 + allowed_goal_duration_margin: 0.5 + allowed_start_tolerance: 0.01 + +moveit_controller_manager: moveit_ros_control_interface/MoveItMultiControllerManager diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/config/moveit_cpp.yaml b/smacc2_sm_reference_library/sm_multi_panda_sim/config/moveit_cpp.yaml new file mode 100644 index 000000000..ffa333f8a --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/config/moveit_cpp.yaml @@ -0,0 +1,18 @@ +planning_scene_monitor_options: + name: "planning_scene_monitor" + robot_description: "robot_description" + joint_state_topic: "/joint_states" + attached_collision_object_topic: "/moveit_cpp/planning_scene_monitor" + publish_planning_scene_topic: "/moveit_cpp/publish_planning_scene" + monitored_planning_scene_topic: "/moveit_cpp/monitored_planning_scene" + wait_for_initial_state_timeout: 10.0 + +planning_pipelines: + #namespace: "moveit_cpp" # optional, default is ~ + pipeline_names: ["ompl"] + +plan_request_params: + planning_attempts: 1 + planning_pipeline: ompl + max_velocity_scaling_factor: 1.0 + max_acceleration_scaling_factor: 1.0 diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/config/ompl_planning.yaml b/smacc2_sm_reference_library/sm_multi_panda_sim/config/ompl_planning.yaml new file mode 100644 index 000000000..d145553d0 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/config/ompl_planning.yaml @@ -0,0 +1,212 @@ +planning_plugin: ompl_interface/OMPLPlanner +request_adapters: >- + default_planner_request_adapters/AddTimeOptimalParameterization + default_planner_request_adapters/ResolveConstraintFrames + default_planner_request_adapters/FixWorkspaceBounds + default_planner_request_adapters/FixStartStateBounds + default_planner_request_adapters/FixStartStateCollision + default_planner_request_adapters/FixStartStatePathConstraints +start_state_max_bounds_error: 0.1 +planner_configs: + SBLkConfigDefault: + type: geometric::SBL + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ESTkConfigDefault: + type: geometric::EST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LBKPIECEkConfigDefault: + type: geometric::LBKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + BKPIECEkConfigDefault: + type: geometric::BKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + KPIECEkConfigDefault: + type: geometric::KPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + RRTkConfigDefault: + type: geometric::RRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + RRTConnectkConfigDefault: + type: geometric::RRTConnect + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + RRTstarkConfigDefault: + type: geometric::RRTstar + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 + TRRTkConfigDefault: + type: geometric::TRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + max_states_failed: 10 # when to start increasing temp. default: 10 + temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 + min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 + init_temperature: 10e-6 # initial temperature. default: 10e-6 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + k_constant: 0.0 # value used to normalize expression. default: 0.0 set in setup() + PRMkConfigDefault: + type: geometric::PRM + max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 + PRMstarkConfigDefault: + type: geometric::PRMstar + FMTkConfigDefault: + type: geometric::FMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1 + nearest_k: 1 # use Knearest strategy. default: 1 + cache_cc: 1 # use collision checking cache. default: 1 + heuristics: 0 # activate cost to go heuristics. default: 0 + extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + BFMTkConfigDefault: + type: geometric::BFMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0 + nearest_k: 1 # use the Knearest strategy. default: 1 + balanced: 0 # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1 + optimality: 1 # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1 + heuristics: 1 # activates cost to go heuristics. default: 1 + cache_cc: 1 # use the collision checking cache. default: 1 + extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + PDSTkConfigDefault: + type: geometric::PDST + STRIDEkConfigDefault: + type: geometric::STRIDE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + use_projected_distance: 0 # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0 + degree: 16 # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16 + max_degree: 18 # max degree of a node in the GNAT. default: 12 + min_degree: 12 # min degree of a node in the GNAT. default: 12 + max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6 + estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0 + min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2 + BiTRRTkConfigDefault: + type: geometric::BiTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1 + init_temperature: 100 # initial temperature. default: 100 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf + LBTRRTkConfigDefault: + type: geometric::LBTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + epsilon: 0.4 # optimality approximation factor. default: 0.4 + BiESTkConfigDefault: + type: geometric::BiEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ProjESTkConfigDefault: + type: geometric::ProjEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LazyPRMkConfigDefault: + type: geometric::LazyPRM + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + LazyPRMstarkConfigDefault: + type: geometric::LazyPRMstar + SPARSkConfigDefault: + type: geometric::SPARS + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 1000 # maximum consecutive failure limit. default: 1000 + SPARStwokConfigDefault: + type: geometric::SPARStwo + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 5000 # maximum consecutive failure limit. default: 5000 + TrajOptDefault: + type: geometric::TrajOpt + +panda_arm: + planner_configs: + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + - FMTkConfigDefault + - BFMTkConfigDefault + - PDSTkConfigDefault + - STRIDEkConfigDefault + - BiTRRTkConfigDefault + - LBTRRTkConfigDefault + - BiESTkConfigDefault + - ProjESTkConfigDefault + - LazyPRMkConfigDefault + - LazyPRMstarkConfigDefault + - SPARSkConfigDefault + - SPARStwokConfigDefault + - TrajOptDefault +panda_arm_hand: + planner_configs: + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + - FMTkConfigDefault + - BFMTkConfigDefault + - PDSTkConfigDefault + - STRIDEkConfigDefault + - BiTRRTkConfigDefault + - LBTRRTkConfigDefault + - BiESTkConfigDefault + - ProjESTkConfigDefault + - LazyPRMkConfigDefault + - LazyPRMstarkConfigDefault + - SPARSkConfigDefault + - SPARStwokConfigDefault + - TrajOptDefault +hand: + planner_configs: + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + - FMTkConfigDefault + - BFMTkConfigDefault + - PDSTkConfigDefault + - STRIDEkConfigDefault + - BiTRRTkConfigDefault + - LBTRRTkConfigDefault + - BiESTkConfigDefault + - ProjESTkConfigDefault + - LazyPRMkConfigDefault + - LazyPRMstarkConfigDefault + - SPARSkConfigDefault + - SPARStwokConfigDefault + - TrajOptDefault diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/config/panda.ros2_control.xacro b/smacc2_sm_reference_library/sm_multi_panda_sim/config/panda.ros2_control.xacro new file mode 100644 index 000000000..3d09e724a --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/config/panda.ros2_control.xacro @@ -0,0 +1,62 @@ + + + + + + + + + fake_components/GenericSystem + + + + + ${initial_positions['panda_joint1']} + + + + + + + ${initial_positions['panda_joint2']} + + + + + + + ${initial_positions['panda_joint3']} + + + + + + + ${initial_positions['panda_joint4']} + + + + + + + ${initial_positions['panda_joint5']} + + + + + + + ${initial_positions['panda_joint6']} + + + + + + + ${initial_positions['panda_joint7']} + + + + + + diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/config/panda.srdf b/smacc2_sm_reference_library/sm_multi_panda_sim/config/panda.srdf new file mode 100644 index 000000000..4120d01ba --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/config/panda.srdf @@ -0,0 +1,149 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/config/panda.urdf.xacro b/smacc2_sm_reference_library/sm_multi_panda_sim/config/panda.urdf.xacro new file mode 100644 index 000000000..eae3a963e --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/config/panda.urdf.xacro @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/config/panda_arm_macro.xacro b/smacc2_sm_reference_library/sm_multi_panda_sim/config/panda_arm_macro.xacro new file mode 100644 index 000000000..df6785149 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/config/panda_arm_macro.xacro @@ -0,0 +1,238 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/config/panda_hand.ros2_control.xacro b/smacc2_sm_reference_library/sm_multi_panda_sim/config/panda_hand.ros2_control.xacro new file mode 100644 index 000000000..9a33778e8 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/config/panda_hand.ros2_control.xacro @@ -0,0 +1,28 @@ + + + + + + + fake_components/GenericSystem + + + + + 0.0 + + + + + ${prefix}panda_finger_joint1 + 1 + + + 0.0 + + + + + + + diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/config/right_initial_positions.yaml b/smacc2_sm_reference_library/sm_multi_panda_sim/config/right_initial_positions.yaml new file mode 100644 index 000000000..b81bb6029 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/config/right_initial_positions.yaml @@ -0,0 +1,9 @@ +# Default initial positions for the panda arm's ros2_control fake system +initial_positions: + panda_joint1: 0.0 + panda_joint2: -0.785 + panda_joint3: 0.0 + panda_joint4: -2.356 + panda_joint5: 0.0 + panda_joint6: 1.571 + panda_joint7: 0.785 diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/config/ros2_controllers.yaml b/smacc2_sm_reference_library/sm_multi_panda_sim/config/ros2_controllers.yaml new file mode 100644 index 000000000..0f8896802 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/config/ros2_controllers.yaml @@ -0,0 +1,46 @@ +# This config file is used by ros2_control +controller_manager: + ros__parameters: + update_rate: 100 # Hz + + left_arm_controller: + type: joint_trajectory_controller/JointTrajectoryController + + right_arm_controller: + type: joint_trajectory_controller/JointTrajectoryController + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + +left_arm_controller: + ros__parameters: + command_interfaces: + - position + state_interfaces: + - position + - velocity + joints: + - left_panda_joint1 + - left_panda_joint2 + - left_panda_joint3 + - left_panda_joint4 + - left_panda_joint5 + - left_panda_joint6 + - left_panda_joint7 + +right_arm_controller: + ros__parameters: + command_interfaces: + - position + state_interfaces: + - position + - velocity + joints: + - right_panda_joint1 + - right_panda_joint2 + - right_panda_joint3 + - right_panda_joint4 + - right_panda_joint5 + - right_panda_joint6 + - right_panda_joint7 diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/clients/move_group/client_behaviors/cb_move_synchronized_link_goals.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/clients/move_group/client_behaviors/cb_move_synchronized_link_goals.hpp new file mode 100644 index 000000000..dbe41ca5f --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/clients/move_group/client_behaviors/cb_move_synchronized_link_goals.hpp @@ -0,0 +1,80 @@ + +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#pragma once + +#include +#include +#include + +#include +#include +#include +#include + +namespace cl_move_group_interface +{ +class CbMoveSynchronizedLinkGoals : public smacc2::SmaccAsyncClientBehavior +{ +public: + CbMoveSynchronizedLinkGoals(const std::vector& poses, const std::vector& names) + { + + } + + void onEntry() override + { + ClMoveGroup * movegroupClient_; + requiresClient(movegroupClient_); + + //auto group_name = movegroupClient_->getOptions().group_name_; + std::string ns = movegroupClient_->getOptions().move_group_namespace_; + + //auto topicname = "/joint_state_broadcaster_"UR5PREFIX"/joint_states"; + + std::string topicname = "/joint_states"; + if (!ns.empty()) + { + topicname = ns + "/" + topicname; + } + + //rclcpp::SensorDataQoS qos; + rclcpp::SubscriptionOptions sub_option; + + sub_ = getNode()->create_subscription( + topicname, 20, std::bind(&CbMoveSynchronizedLinkGoals::onMessageReceived, this, std::placeholders::_1), + sub_option); + + mutex.lock(); + } + +private: + void onMessageReceived(const sensor_msgs::msg::JointState & msg) + { + postSuccessEvent(); + mutex.unlock(); + sub_ = nullptr; // unsuscribe + } + + bool triggered = false; + std::mutex mutex; + rclcpp::Subscription::SharedPtr sub_; +}; +} diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/clients/move_group/client_behaviors/cb_wait_joint_states.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/clients/move_group/client_behaviors/cb_wait_joint_states.hpp new file mode 100644 index 000000000..e9985347e --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/clients/move_group/client_behaviors/cb_wait_joint_states.hpp @@ -0,0 +1,78 @@ + +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#pragma once + +#include +#include +#include + +#include +#include + +namespace cl_move_group_interface +{ +class CbWaitJointState : public smacc2::SmaccAsyncClientBehavior +{ +public: + CbWaitJointState() + { + + } + + void onEntry() override + { + ClMoveGroup * movegroupClient_; + requiresClient(movegroupClient_); + + //auto group_name = movegroupClient_->getOptions().group_name_; + std::string ns = movegroupClient_->getOptions().move_group_namespace_; + + //auto topicname = "/joint_state_broadcaster_"UR5PREFIX"/joint_states"; + + std::string topicname = "/joint_states"; + if (!ns.empty()) + { + topicname = ns + "/" + topicname; + } + + //rclcpp::SensorDataQoS qos; + rclcpp::SubscriptionOptions sub_option; + + sub_ = getNode()->create_subscription( + topicname, 20, std::bind(&CbWaitJointState::onMessageReceived, this, std::placeholders::_1), + sub_option); + + mutex.lock(); + } + +private: + void onMessageReceived(const sensor_msgs::msg::JointState & msg) + { + postSuccessEvent(); + mutex.unlock(); + sub_ = nullptr; // unsuscribe + } + + bool triggered = false; + std::mutex mutex; + rclcpp::Subscription::SharedPtr sub_; +}; +} diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/domain_states/ds_clearing.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/domain_states/ds_clearing.hpp new file mode 100644 index 000000000..1628b8807 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/domain_states/ds_clearing.hpp @@ -0,0 +1,35 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +// STATE DECLARATION +struct DsClearing : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + smacc2::Transition + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} +}; + +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/domain_states/ds_run.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/domain_states/ds_run.hpp new file mode 100644 index 000000000..e77cb3445 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/domain_states/ds_run.hpp @@ -0,0 +1,36 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +// STATE DECLARATION +struct DsRun : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + smacc2::Transition + + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} +}; + +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/domain_states/ds_stopped.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/domain_states/ds_stopped.hpp new file mode 100644 index 000000000..0382d2894 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/domain_states/ds_stopped.hpp @@ -0,0 +1,36 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +// STATE DECLARATION +struct DsStopped : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + //smacc2::Transition + smacc2::Transition + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} +}; + +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/domain_states/ds_stopping.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/domain_states/ds_stopping.hpp new file mode 100644 index 000000000..3d39e846a --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/domain_states/ds_stopping.hpp @@ -0,0 +1,35 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +// STATE DECLARATION +struct DsStopping : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + smacc2::Transition + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} +}; + +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/j_states/js_aborted.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/j_states/js_aborted.hpp new file mode 100644 index 000000000..dad542963 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/j_states/js_aborted.hpp @@ -0,0 +1,35 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +// STATE DECLARATION +struct JsAborted : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + smacc2::Transition + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} +}; + +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/j_states/js_aborting.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/j_states/js_aborting.hpp new file mode 100644 index 000000000..c79e66cfb --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/j_states/js_aborting.hpp @@ -0,0 +1,35 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +// STATE DECLARATION +struct JsAborting : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + smacc2::Transition + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} +}; + +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/j_states/js_active.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/j_states/js_active.hpp new file mode 100644 index 000000000..e1ecefa03 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/j_states/js_active.hpp @@ -0,0 +1,35 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +// STATE DECLARATION +struct JsActive : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} +}; + +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/mode_states/ms_complete.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/mode_states/ms_complete.hpp new file mode 100644 index 000000000..5784e56c9 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/mode_states/ms_complete.hpp @@ -0,0 +1,36 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +// STATE DECLARATION +struct MsComplete : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + smacc2::Transition + + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} +}; + +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/mode_states/ms_completing.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/mode_states/ms_completing.hpp new file mode 100644 index 000000000..a5bdd16e1 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/mode_states/ms_completing.hpp @@ -0,0 +1,43 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +// STATE DECLARATION +struct MsCompleting : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} + + + static constexpr int ztotal_iterations() { return 1; } + int ziteration_count = 0; + + + static constexpr int ytotal_iterations() { return 1; } + int yiteration_count = 0; +}; + +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/mode_states/ms_execute.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/mode_states/ms_execute.hpp new file mode 100644 index 000000000..06ea50278 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/mode_states/ms_execute.hpp @@ -0,0 +1,45 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +// STATE DECLARATION +struct MsExecute : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + smacc2::Transition, + smacc2::Transition, + smacc2::Transition + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} + + + static constexpr int ztotal_iterations() { return 1; } + int ziteration_count = 0; + + + static constexpr int ytotal_iterations() { return 1; } + int yiteration_count = 0; +}; + +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/mode_states/ms_held.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/mode_states/ms_held.hpp new file mode 100644 index 000000000..e23ec62d1 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/mode_states/ms_held.hpp @@ -0,0 +1,35 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +// STATE DECLARATION +struct MsHeld : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + smacc2::Transition + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} +}; + +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/mode_states/ms_holding.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/mode_states/ms_holding.hpp new file mode 100644 index 000000000..63b1ccf17 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/mode_states/ms_holding.hpp @@ -0,0 +1,44 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +// STATE DECLARATION +struct MsHolding : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + smacc2::Transition + + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} + + + static constexpr int ztotal_iterations() { return 1; } + int ziteration_count = 0; + + + static constexpr int ytotal_iterations() { return 1; } + int yiteration_count = 0; +}; + +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/mode_states/ms_idle.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/mode_states/ms_idle.hpp new file mode 100644 index 000000000..e73de60a9 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/mode_states/ms_idle.hpp @@ -0,0 +1,35 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +// STATE DECLARATION +struct MsIdle : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + smacc2::Transition + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} +}; + +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/mode_states/ms_resetting.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/mode_states/ms_resetting.hpp new file mode 100644 index 000000000..3be0b2de4 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/mode_states/ms_resetting.hpp @@ -0,0 +1,35 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +// STATE DECLARATION +struct MsResetting : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + smacc2::Transition + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} +}; + +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/mode_states/ms_starting.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/mode_states/ms_starting.hpp new file mode 100644 index 000000000..72f3b13b6 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/mode_states/ms_starting.hpp @@ -0,0 +1,44 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +// STATE DECLARATION +struct MsStarting : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + smacc2::Transition + + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} + + + static constexpr int ztotal_iterations() { return 1; } + int ziteration_count = 0; + + + static constexpr int ytotal_iterations() { return 1; } + int yiteration_count = 0; +}; + +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/mode_states/ms_suspended.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/mode_states/ms_suspended.hpp new file mode 100644 index 000000000..8dd30816d --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/mode_states/ms_suspended.hpp @@ -0,0 +1,43 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +// STATE DECLARATION +struct MsSuspended : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + smacc2::Transition + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} + + + static constexpr int ztotal_iterations() { return 1; } + int ziteration_count = 0; + + + static constexpr int ytotal_iterations() { return 1; } + int yiteration_count = 0; +}; + +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/mode_states/ms_suspending.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/mode_states/ms_suspending.hpp new file mode 100644 index 000000000..a65f9c736 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/mode_states/ms_suspending.hpp @@ -0,0 +1,50 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +// STATE DECLARATION +struct MsSuspending : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + smacc2::Transition + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} + + + static constexpr int ztotal_iterations() { return 1; } + int ziteration_count = 0; + + + static constexpr int ytotal_iterations() { return 1; } + int yiteration_count = 0; + + + static constexpr int dtotal_iterations() { return 1; } + int diteration_count = 0; + + static constexpr int gtotal_iterations() { return 1; } + int giteration_count = 0; +}; + +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/mode_states/ms_unholding.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/mode_states/ms_unholding.hpp new file mode 100644 index 000000000..0847a1e0a --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/mode_states/ms_unholding.hpp @@ -0,0 +1,36 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +// STATE DECLARATION +struct MsUnholding : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + smacc2::Transition + + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} +}; + +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/mode_states/ms_unsuspending.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/mode_states/ms_unsuspending.hpp new file mode 100644 index 000000000..26c0c96cb --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/mode_states/ms_unsuspending.hpp @@ -0,0 +1,35 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +// STATE DECLARATION +struct MsUnsuspending : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} +}; + +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/orthogonals/or_arm_left.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/orthogonals/or_arm_left.hpp new file mode 100644 index 000000000..9099d8920 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/orthogonals/or_arm_left.hpp @@ -0,0 +1,51 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include + +#include +#include +#include + +#include "ros_timer_client/cl_ros_timer.hpp" +#include "smacc2/smacc.hpp" + +namespace sm_multi_panda_sim +{ +using namespace std::chrono_literals; + +class OrArmLeft : public smacc2::Orthogonal +{ +public: + void onInitialize() override + { + //moveit::planning_interface::MoveGroupInterface::Options options ("left_panda_arm"); + + // right + //moveit::planning_interface::MoveGroupInterface::Options options ("right_panda_arm"); + + moveit::planning_interface::MoveGroupInterface::Options options ("both_panda_arms"); + + + auto move_group_client = this->createClient(options); //ur_manipulator + + move_group_client->createComponent(); + + auto graspingComponent = move_group_client->createComponent(); + graspingComponent->gripperLink_="tool0"; + } +}; +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/orthogonals/or_arm_right.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/orthogonals/or_arm_right.hpp new file mode 100644 index 000000000..114f84cba --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/orthogonals/or_arm_right.hpp @@ -0,0 +1,46 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include + +#include +#include +#include + +#include "ros_timer_client/cl_ros_timer.hpp" +#include "smacc2/smacc.hpp" + +namespace sm_multi_panda_sim +{ +using namespace std::chrono_literals; + +class OrArmRight : public smacc2::Orthogonal +{ +public: + void onInitialize() override + { + + moveit::planning_interface::MoveGroupInterface::Options options ("right_panda_arm"); + + auto move_group_client = this->createClient(options); //ur_manipulator + + move_group_client->createComponent(); + + auto graspingComponent = move_group_client->createComponent(); + graspingComponent->gripperLink_="tool0"; + } +}; +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/orthogonals/or_keyboard.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/orthogonals/or_keyboard.hpp new file mode 100644 index 000000000..1d886cc54 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/orthogonals/or_keyboard.hpp @@ -0,0 +1,30 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include + +namespace sm_multi_panda_sim +{ +class OrKeyboard : public smacc2::Orthogonal +{ +public: + virtual void onInitialize() override + { + auto clKeyboard = this->createClient(); + } +}; +} // namespace sm_pack_ml diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/orthogonals/or_timer.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/orthogonals/or_timer.hpp new file mode 100644 index 000000000..d432af051 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/orthogonals/or_timer.hpp @@ -0,0 +1,32 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include +#include + +namespace sm_multi_panda_sim +{ +using namespace std::chrono_literals; +class OrTimer : public smacc2::Orthogonal +{ +public: + virtual void onInitialize() override + { + auto actionclient = this->createClient(rclcpp::Duration(0.1s)); + } +}; +} // namespace sm_pack_ml diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/orthogonals/or_updatable_publisher.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/orthogonals/or_updatable_publisher.hpp new file mode 100644 index 000000000..c868c6de1 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/orthogonals/or_updatable_publisher.hpp @@ -0,0 +1,33 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include +#include + +namespace sm_multi_panda_sim +{ +using namespace cl_ros_publisher; + +class OrUpdatablePublisher : public smacc2::Orthogonal +{ +public: + virtual void onInitialize() override + { + auto ros_publisher_client = this->createClient(); + } +}; +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/sm_multi_panda_sim.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/sm_multi_panda_sim.hpp new file mode 100644 index 000000000..f73e54007 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/sm_multi_panda_sim.hpp @@ -0,0 +1,616 @@ +// Copyright 2021 MyName/MyCompany Inc. +// Copyright 2021 RobosoftAI Inc. (template) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + *****************************************************************************************************************/ + +#pragma once + +#include + +#include "rclcpp/rclcpp.hpp" +#include "smacc2/smacc.hpp" +#include + +//CLIENT BEHAVIORS +#include +#include +#include + +// #include +// #include + +#include + +//#include +#include + +//STATE REACTORS +#include + +// ORTHOGONALS +#include "orthogonals/or_arm_left.hpp" +#include "orthogonals/or_arm_right.hpp" +#include "orthogonals/or_timer.hpp" +#include "orthogonals/or_keyboard.hpp" +#include "orthogonals/or_updatable_publisher.hpp" + +// CLIENTS +#include +#include + +// CLIENT BEHAVIORS +#include +#include "clients/move_group/client_behaviors/cb_wait_joint_states.hpp" +#include "clients/move_group/client_behaviors/cb_move_synchronized_link_goals.hpp" + +namespace sm_multi_panda_sim +{ + +using namespace cl_move_group_interface; +using namespace cl_ros_timer; +using namespace cl_keyboard; +using namespace boost; +using namespace smacc2; + +using namespace smacc2::default_events; + +//STATES +struct StAcquireSensors; +struct StMoveJoints; + +class JsAborting; +class JsActive; +class JsAborted; + +class DsClearing; +class DsRun; +class DsStopped; +class DsStopping; + +class MsComplete; +class MsCompleting; +class MsExecute; +class MsHeld; +class MsHolding; +class MsIdle; +class MsResetting; +class MsStarting; +class MsSuspended; +class MsSuspending; +class MsUnholding; +class MsUnsuspending; + +//SUPERSTATES +class SsExecuteSequenceA; +namespace execute_sequence_a +{ +class StiExecuteSequenceALoop; +class StiExecuteSequenceAStep1; +class StiExecuteSequenceAStep2; +class StiExecuteSequenceAStep3; +class StiExecuteSequenceAStep4; +class StiExecuteSequenceAStep5; +class StiExecuteSequenceAStep6; +class StiExecuteSequenceAStep7; +class StiExecuteSequenceAStep8; +class StiExecuteSequenceAStep9; +} // namespace execute_sequence_a + +class SsStartSequenceA; +namespace start_sequence_a +{ +class StiStartSequenceALoop; +class StiStartSequenceAStep1; +class StiStartSequenceAStep2; +class StiStartSequenceAStep3; +class StiStartSequenceAStep4; +class StiStartSequenceAStep5; +class StiStartSequenceAStep6; +class StiStartSequenceAStep7; +class StiStartSequenceAStep8; +class StiStartSequenceAStep9; +} // namespace start_sequence_a + +class SsCompletingSequenceA; +namespace completing_sequence_a +{ +class StiCompletingSequenceALoop; +class StiCompletingSequenceAStep1; +class StiCompletingSequenceAStep2; +class StiCompletingSequenceAStep3; +class StiCompletingSequenceAStep4; +class StiCompletingSequenceAStep5; +class StiCompletingSequenceAStep6; +class StiCompletingSequenceAStep7; +class StiCompletingSequenceAStep8; +class StiCompletingSequenceAStep9; +} // namespace completing_sequence_a + +class SsSuspendingSequenceA; +namespace suspending_sequence_a +{ +class StiSuspendingSequenceALoop; +class StiSuspendingSequenceAStep1; +class StiSuspendingSequenceAStep2; +class StiSuspendingSequenceAStep3; +class StiSuspendingSequenceAStep4; +class StiSuspendingSequenceAStep5; +class StiSuspendingSequenceAStep6; +class StiSuspendingSequenceAStep7; +class StiSuspendingSequenceAStep8; +class StiSuspendingSequenceAStep9; +} // namespace suspending_sequence_a + +class SsSuspendingSequenceC; +namespace suspending_sequence_c +{ +class StiSuspendingSequenceCLoop; +class StiSuspendingSequenceCStep1; +class StiSuspendingSequenceCStep2; +class StiSuspendingSequenceCStep3; +class StiSuspendingSequenceCStep4; +class StiSuspendingSequenceCStep5; +class StiSuspendingSequenceCStep6; +class StiSuspendingSequenceCStep7; +class StiSuspendingSequenceCStep8; +class StiSuspendingSequenceCStep9; +} // namespace suspending_sequence_c + +class SsSuspendingSequenceD; +namespace suspending_sequence_d +{ +class StiSuspendingSequenceDLoop; +class StiSuspendingSequenceDStep1; +class StiSuspendingSequenceDStep2; +class StiSuspendingSequenceDStep3; +class StiSuspendingSequenceDStep4; +class StiSuspendingSequenceDStep5; +class StiSuspendingSequenceDStep6; +class StiSuspendingSequenceDStep7; +class StiSuspendingSequenceDStep8; +class StiSuspendingSequenceDStep9; +} // namespace suspending_sequence_d + +class SsHoldingSequenceA; +namespace holding_sequence_a +{ +class StiHoldingSequenceALoop; +class StiHoldingSequenceAStep1; +class StiHoldingSequenceAStep2; +class StiHoldingSequenceAStep3; +class StiHoldingSequenceAStep4; +class StiHoldingSequenceAStep5; +class StiHoldingSequenceAStep6; +class StiHoldingSequenceAStep7; +class StiHoldingSequenceAStep8; +class StiHoldingSequenceAStep9; +} // namespace holding_sequence_a + + +class SsExecuteSequenceB; +namespace execute_sequence_b +{ +//FORWARD DECLARATIONS OF ALL INNER STATES +class StiExecuteSequenceBLoop; +class StiExecuteSequenceBStep1; +class StiExecuteSequenceBStep2; +class StiExecuteSequenceBStep3; +class StiExecuteSequenceBStep4; +class StiExecuteSequenceBStep5; +class StiExecuteSequenceBStep6; +class StiExecuteSequenceBStep7; +class StiExecuteSequenceBStep8; +class StiExecuteSequenceBStep9; +} // namespace execute_sequence_b + +class SsStartSequenceB; + +namespace start_sequence_b +{ +//FORWARD DECLARATIONS OF ALL INNER STATES +class StiStartSequenceBLoop; +class StiStartSequenceBStep1; +class StiStartSequenceBStep2; +class StiStartSequenceBStep3; +class StiStartSequenceBStep4; +class StiStartSequenceBStep5; +class StiStartSequenceBStep6; +class StiStartSequenceBStep7; +class StiStartSequenceBStep8; +class StiStartSequenceBStep9; +} // namespace start_sequence_b + +class SsCompletingSequenceB; + +namespace completing_sequence_b +{ +//FORWARD DECLARATIONS OF ALL INNER STATES +class StiCompletingSequenceBLoop; +class StiCompletingSequenceBStep1; +class StiCompletingSequenceBStep2; +class StiCompletingSequenceBStep3; +class StiCompletingSequenceBStep4; +class StiCompletingSequenceBStep5; +class StiCompletingSequenceBStep6; +class StiCompletingSequenceBStep7; +class StiCompletingSequenceBStep8; +class StiCompletingSequenceBStep9; +} // namespace completing_sequence_b + +class SsSuspendingSequenceB; + +namespace suspending_sequence_b +{ +//FORWARD DECLARATIONS OF ALL INNER STATES +class StiSuspendingSequenceB; +class StiSuspendingSequenceBStep1; +class StiSuspendingSequenceBStep2; +class StiSuspendingSequenceBStep3; +class StiSuspendingSequenceBStep4; +class StiSuspendingSequenceBStep5; +class StiSuspendingSequenceBStep6; +class StiSuspendingSequenceBStep7; +class StiSuspendingSequenceBStep8; +class StiSuspendingSequenceBStep9; +} // namespace suspending_sequence_b + +class SsHoldingSequenceB; + +namespace holding_sequence_b +{ +//FORWARD DECLARATIONS OF ALL INNER STATES +class StiHoldingSequenceBLoop; +class StiHoldingSequenceBStep1; +class StiHoldingSequenceBStep2; +class StiHoldingSequenceBStep3; +class StiHoldingSequenceBStep4; +class StiHoldingSequenceBStep5; +class StiHoldingSequenceBStep6; +class StiHoldingSequenceBStep7; +class StiHoldingSequenceBStep8; +class StiHoldingSequenceBStep9; +} // namespace holding_sequence_b + +//STATES +class ExecuteStObserve; +class StRecoveryAnalyze1; +class StRecoveryBifurcate1; +class StRecoveryCalculate1; +class StRecoveryDeliberate1; +class StRecoveryEvaluate1; +class StRecoveryGenerate1; +class StRecoveryInnervate1; + +class StartStObserve; +class StRecoveryAnalyze2; +class StRecoveryBifurcate2; +class StRecoveryCalculate2; +class StRecoveryDeliberate2; +class StRecoveryEvaluate2; +class StRecoveryGenerate2; +class StRecoveryInnervate2; + +class CompletingStObserve; +class SuspendingStObserve; +class HoldingStObserve; + + +class ExecuteSequenceALoop; +class ExecuteSequenceBLoop; +class StartSequenceALoop; +class StartSequenceBLoop; +class CompletingSequenceALoop; +class CompletingSequenceBLoop; +class SuspendingSequenceALoop; +class SuspendingSequenceCLoop; +class SuspendingSequenceDLoop; +class SuspendingSequenceBLoop; +class HoldingSequenceALoop; +class HoldingSequenceBLoop; + + +//MODE STATES +class MsExecute; +class MsRecovery1; + +//MODE STATES +class MsStarting; +class MsRecovery2; + +class MsCompleting; +class MsSuspending; +class MsHolding; + +struct EvToDeep : sc::event +{ +}; + +struct EvFail : sc::event +{ +}; + +struct EvSc : sc::event +{ +}; + +struct EvStart : sc::event +{ +}; + +struct EvReset : sc::event +{ +}; + +struct EvSuspend : sc::event +{ +}; + +struct EvUnSuspend : sc::event +{ +}; + +struct EvHold : sc::event +{ +}; + +struct EvUnhold:sc::event +{ +}; + +struct EvStop:sc::event +{ +}; + +struct EvClear:sc::event +{ +}; + +//-------------------------------------------------------------------- +//STATE_MACHINE +//struct SmMultiPandaSim : public smacc2::SmaccStateMachineBase + +struct SmMultiPandaSim : public smacc2::SmaccStateMachineBase +{ + using SmaccStateMachineBase::SmaccStateMachineBase; + + void onInitialize() override + { + this->createOrthogonal(); + this->createOrthogonal(); + } +}; + +} // namespace sm_multi_panda_sim + + + + + + +#include "j_states/js_aborted.hpp" +#include "j_states/js_aborting.hpp" +#include "j_states/js_active.hpp" + +#include "domain_states/ds_clearing.hpp" +#include "domain_states/ds_run.hpp" +#include "domain_states/ds_stopped.hpp" +#include "domain_states/ds_stopping.hpp" + +#include "mode_states/ms_complete.hpp" +#include "mode_states/ms_completing.hpp" +#include "mode_states/ms_execute.hpp" +#include "mode_states/ms_held.hpp" +#include "mode_states/ms_holding.hpp" +#include "mode_states/ms_idle.hpp" +#include "mode_states/ms_resetting.hpp" +#include "mode_states/ms_starting.hpp" +#include "mode_states/ms_suspended.hpp" +#include "mode_states/ms_suspending.hpp" +#include "mode_states/ms_unholding.hpp" +#include "mode_states/ms_unsuspending.hpp" + +#include "states/execute_sequence_a_loop.hpp" +#include "states/start_sequence_a_loop.hpp" + + +// STATES +#include "states/st_acquire_sensors.hpp" +#include "states/st_move_joints.hpp" + + +#include "states/completing_sequence_a_loop.hpp" +#include "states/suspending_sequence_a_loop.hpp" +#include "states/holding_sequence_a_loop.hpp" + +#include "states/suspending_sequence_c_loop.hpp" +#include "states/suspending_sequence_d_loop.hpp" + +#include "states/execute_sequence_b_loop.hpp" +#include "states/start_sequence_b_loop.hpp" +#include "states/completing_sequence_b_loop.hpp" +#include "states/suspending_sequence_b_loop.hpp" +#include "states/holding_sequence_b_loop.hpp" + +#include "states/execute_st_observe.hpp" +#include "states/start_st_observe.hpp" +#include "states/completing_st_observe.hpp" +#include "states/suspending_st_observe.hpp" +#include "states/holding_st_observe.hpp" + +#include "superstates/ss_execute_sequence_a.hpp" +#include "superstates/ss_start_sequence_a.hpp" +#include "superstates/ss_completing_sequence_a.hpp" +#include "superstates/ss_suspending_sequence_a.hpp" +#include "superstates/ss_holding_sequence_a.hpp" + +#include "superstates/ss_suspending_sequence_c.hpp" +#include "superstates/ss_suspending_sequence_d.hpp" + +#include "superstates/ss_execute_sequence_b.hpp" +#include "superstates/ss_start_sequence_b.hpp" +#include "superstates/ss_completing_sequence_b.hpp" +#include "superstates/ss_suspending_sequence_b.hpp" +#include "superstates/ss_holding_sequence_b.hpp" + +// //ss_start_sequence_a +#include "states/start_sequence_a/sti_start_sequence_a_loop.hpp" + +#include "states/start_sequence_a/sti_start_sequence_a_step_4.hpp" +#include "states/start_sequence_a/sti_start_sequence_a_step_3.hpp" +#include "states/start_sequence_a/sti_start_sequence_a_step_1.hpp" +#include "states/start_sequence_a/sti_start_sequence_a_step_2.hpp" +#include "states/start_sequence_a/sti_start_sequence_a_step_5.hpp" +#include "states/start_sequence_a/sti_start_sequence_a_step_9.hpp" +#include "states/start_sequence_a/sti_start_sequence_a_step_8.hpp" +#include "states/start_sequence_a/sti_start_sequence_a_step_7.hpp" +#include "states/start_sequence_a/sti_start_sequence_a_step_6.hpp" + +// //ss_execute_sequence_a +#include "states/execute_sequence_a/sti_execute_sequence_a_step_4.hpp" +#include "states/execute_sequence_a/sti_execute_sequence_a_step_3.hpp" +#include "states/execute_sequence_a/sti_execute_sequence_a_step_1.hpp" +#include "states/execute_sequence_a/sti_execute_sequence_a_loop.hpp" +#include "states/execute_sequence_a/sti_execute_sequence_a_step_2.hpp" +#include "states/execute_sequence_a/sti_execute_sequence_a_step_5.hpp" +#include "states/execute_sequence_a/sti_execute_sequence_a_step_9.hpp" +#include "states/execute_sequence_a/sti_execute_sequence_a_step_8.hpp" +#include "states/execute_sequence_a/sti_execute_sequence_a_step_7.hpp" +#include "states/execute_sequence_a/sti_execute_sequence_a_step_6.hpp" + + +//ss_completing_sequence_a +#include "states/completing_sequence_a/sti_completing_sequence_a_step_4.hpp" +#include "states/completing_sequence_a/sti_completing_sequence_a_step_3.hpp" +#include "states/completing_sequence_a/sti_completing_sequence_a_step_1.hpp" +#include "states/completing_sequence_a/sti_completing_sequence_a_loop.hpp" +#include "states/completing_sequence_a/sti_completing_sequence_a_step_2.hpp" +#include "states/completing_sequence_a/sti_completing_sequence_a_step_5.hpp" +#include "states/completing_sequence_a/sti_completing_sequence_a_step_9.hpp" +#include "states/completing_sequence_a/sti_completing_sequence_a_step_8.hpp" +#include "states/completing_sequence_a/sti_completing_sequence_a_step_7.hpp" +#include "states/completing_sequence_a/sti_completing_sequence_a_step_6.hpp" + +//ss_suspending_sequence_a +#include "states/suspending_sequence_a/sti_suspending_sequence_a_step_4.hpp" +#include "states/suspending_sequence_a/sti_suspending_sequence_a_step_3.hpp" +#include "states/suspending_sequence_a/sti_suspending_sequence_a_step_1.hpp" +#include "states/suspending_sequence_a/sti_suspending_sequence_a_loop.hpp" +#include "states/suspending_sequence_a/sti_suspending_sequence_a_step_2.hpp" +#include "states/suspending_sequence_a/sti_suspending_sequence_a_step_5.hpp" +#include "states/suspending_sequence_a/sti_suspending_sequence_a_step_9.hpp" +#include "states/suspending_sequence_a/sti_suspending_sequence_a_step_8.hpp" +#include "states/suspending_sequence_a/sti_suspending_sequence_a_step_7.hpp" +#include "states/suspending_sequence_a/sti_suspending_sequence_a_step_6.hpp" + +//ss_suspending_sequence_c +#include "states/suspending_sequence_c/sti_suspending_sequence_c_loop.hpp" +#include "states/suspending_sequence_c/sti_suspending_sequence_c_step_1.hpp" +#include "states/suspending_sequence_c/sti_suspending_sequence_c_step_2.hpp" +#include "states/suspending_sequence_c/sti_suspending_sequence_c_step_3.hpp" +#include "states/suspending_sequence_c/sti_suspending_sequence_c_step_4.hpp" +#include "states/suspending_sequence_c/sti_suspending_sequence_c_step_5.hpp" +#include "states/suspending_sequence_c/sti_suspending_sequence_c_step_6.hpp" +#include "states/suspending_sequence_c/sti_suspending_sequence_c_step_7.hpp" +#include "states/suspending_sequence_c/sti_suspending_sequence_c_step_8.hpp" +#include "states/suspending_sequence_c/sti_suspending_sequence_c_step_9.hpp" + +//ss_suspending_sequence_d +#include "states/suspending_sequence_d/sti_suspending_sequence_d_step_4.hpp" +#include "states/suspending_sequence_d/sti_suspending_sequence_d_step_3.hpp" +#include "states/suspending_sequence_d/sti_suspending_sequence_d_step_1.hpp" +#include "states/suspending_sequence_d/sti_suspending_sequence_d_loop.hpp" +#include "states/suspending_sequence_d/sti_suspending_sequence_d_step_2.hpp" +#include "states/suspending_sequence_d/sti_suspending_sequence_d_step_5.hpp" +#include "states/suspending_sequence_d/sti_suspending_sequence_d_step_9.hpp" +#include "states/suspending_sequence_d/sti_suspending_sequence_d_step_8.hpp" +#include "states/suspending_sequence_d/sti_suspending_sequence_d_step_7.hpp" +#include "states/suspending_sequence_d/sti_suspending_sequence_d_step_6.hpp" + +//ss_holding_sequence_a +#include "states/holding_sequence_a/sti_holding_sequence_a_step_4.hpp" +#include "states/holding_sequence_a/sti_holding_sequence_a_step_3.hpp" +#include "states/holding_sequence_a/sti_holding_sequence_a_step_1.hpp" +#include "states/holding_sequence_a/sti_holding_sequence_a_loop.hpp" +#include "states/holding_sequence_a/sti_holding_sequence_a_step_2.hpp" +#include "states/holding_sequence_a/sti_holding_sequence_a_step_5.hpp" +#include "states/holding_sequence_a/sti_holding_sequence_a_step_9.hpp" +#include "states/holding_sequence_a/sti_holding_sequence_a_step_8.hpp" +#include "states/holding_sequence_a/sti_holding_sequence_a_step_7.hpp" +#include "states/holding_sequence_a/sti_holding_sequence_a_step_6.hpp" + +// //ss_execute_sequence_b +#include "states/execute_sequence_b/sti_execute_sequence_b_step_4.hpp" +#include "states/execute_sequence_b/sti_execute_sequence_b_step_3.hpp" +#include "states/execute_sequence_b/sti_execute_sequence_b_step_1.hpp" +#include "states/execute_sequence_b/sti_execute_sequence_b_loop.hpp" +#include "states/execute_sequence_b/sti_execute_sequence_b_step_2.hpp" +#include "states/execute_sequence_b/sti_execute_sequence_b_step_5.hpp" +#include "states/execute_sequence_b/sti_execute_sequence_b_step_9.hpp" +#include "states/execute_sequence_b/sti_execute_sequence_b_step_8.hpp" +#include "states/execute_sequence_b/sti_execute_sequence_b_step_7.hpp" +#include "states/execute_sequence_b/sti_execute_sequence_b_step_6.hpp" + +// //ss_mode_2_sequence_b +#include "states/start_sequence_b/sti_start_sequence_b_step_4.hpp" +#include "states/start_sequence_b/sti_start_sequence_b_step_3.hpp" +#include "states/start_sequence_b/sti_start_sequence_b_step_1.hpp" +#include "states/start_sequence_b/sti_start_sequence_b_loop.hpp" +#include "states/start_sequence_b/sti_start_sequence_b_step_2.hpp" +#include "states/start_sequence_b/sti_start_sequence_b_step_5.hpp" +#include "states/start_sequence_b/sti_start_sequence_b_step_9.hpp" +#include "states/start_sequence_b/sti_start_sequence_b_step_8.hpp" +#include "states/start_sequence_b/sti_start_sequence_b_step_7.hpp" +#include "states/start_sequence_b/sti_start_sequence_b_step_6.hpp" + +//ss_completing_sequence_b +#include "states/completing_sequence_b/sti_completing_sequence_b_step_4.hpp" +#include "states/completing_sequence_b/sti_completing_sequence_b_step_3.hpp" +#include "states/completing_sequence_b/sti_completing_sequence_b_step_1.hpp" +#include "states/completing_sequence_b/sti_completing_sequence_b_loop.hpp" +#include "states/completing_sequence_b/sti_completing_sequence_b_step_2.hpp" +#include "states/completing_sequence_b/sti_completing_sequence_b_step_5.hpp" +#include "states/completing_sequence_b/sti_completing_sequence_b_step_9.hpp" +#include "states/completing_sequence_b/sti_completing_sequence_b_step_8.hpp" +#include "states/completing_sequence_b/sti_completing_sequence_b_step_7.hpp" +#include "states/completing_sequence_b/sti_completing_sequence_b_step_6.hpp" + +//ss_suspending_sequence_b +#include "states/suspending_sequence_b/sti_suspending_sequence_b_step_4.hpp" +#include "states/suspending_sequence_b/sti_suspending_sequence_b_step_3.hpp" +#include "states/suspending_sequence_b/sti_suspending_sequence_b_step_1.hpp" +#include "states/suspending_sequence_b/sti_suspending_sequence_b_loop.hpp" +#include "states/suspending_sequence_b/sti_suspending_sequence_b_step_2.hpp" +#include "states/suspending_sequence_b/sti_suspending_sequence_b_step_5.hpp" +#include "states/suspending_sequence_b/sti_suspending_sequence_b_step_9.hpp" +#include "states/suspending_sequence_b/sti_suspending_sequence_b_step_8.hpp" +#include "states/suspending_sequence_b/sti_suspending_sequence_b_step_7.hpp" +#include "states/suspending_sequence_b/sti_suspending_sequence_b_step_6.hpp" + + +//ss_holding_seqence_b +#include "states/holding_sequence_b/sti_holding_sequence_b_step_4.hpp" +#include "states/holding_sequence_b/sti_holding_sequence_b_step_3.hpp" +#include "states/holding_sequence_b/sti_holding_sequence_b_step_1.hpp" +#include "states/holding_sequence_b/sti_holding_sequence_b_loop.hpp" +#include "states/holding_sequence_b/sti_holding_sequence_b_step_2.hpp" +#include "states/holding_sequence_b/sti_holding_sequence_b_step_5.hpp" +#include "states/holding_sequence_b/sti_holding_sequence_b_step_9.hpp" +#include "states/holding_sequence_b/sti_holding_sequence_b_step_8.hpp" +#include "states/holding_sequence_b/sti_holding_sequence_b_step_7.hpp" +#include "states/holding_sequence_b/sti_holding_sequence_b_step_6.hpp" diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_a/sti_completing_sequence_a_loop.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_a/sti_completing_sequence_a_loop.hpp new file mode 100644 index 000000000..d901c6473 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_a/sti_completing_sequence_a_loop.hpp @@ -0,0 +1,54 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace completing_sequence_a +{ +// STATE DECLARATION +struct StiCompletingSequenceALoop : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiCompletingSequenceAStep1, CONTINUELOOP> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} + + bool loopWhileCondition() + { + auto & superstate = this->context(); + + RCLCPP_INFO( + getLogger(), "Loop start, current iterations: %d, total iterations: %d", + superstate.iteration_count, superstate.total_iterations()); + return superstate.iteration_count++ < superstate.total_iterations(); + } + + void onEntry() + { + RCLCPP_INFO(getLogger(), "LOOP START ON ENTRY"); + checkWhileLoopConditionAndThrowEvent(&StiCompletingSequenceALoop::loopWhileCondition); + } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_a/sti_completing_sequence_a_step_1.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_a/sti_completing_sequence_a_step_1.hpp new file mode 100644 index 000000000..27f3f6814 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_a/sti_completing_sequence_a_step_1.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace completing_sequence_a +{ +// STATE DECLARATION +struct StiCompletingSequenceAStep1 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiCompletingSequenceAStep2, SUCCESS>, + Transition, StiCompletingSequenceAStep3, NEXT> + //Transition, StiStartSequenceALoop, PREVIOUS>, + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_a/sti_completing_sequence_a_step_2.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_a/sti_completing_sequence_a_step_2.hpp new file mode 100644 index 000000000..9748c4f82 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_a/sti_completing_sequence_a_step_2.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace completing_sequence_a +{ +// STATE DECLARATION +struct StiCompletingSequenceAStep2 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiCompletingSequenceAStep3, TIMEOUT>, + Transition, StiCompletingSequenceAStep1, PREVIOUS>, + Transition, StiCompletingSequenceAStep3, NEXT> + + //Transition, ExecuteStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_a/sti_completing_sequence_a_step_3.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_a/sti_completing_sequence_a_step_3.hpp new file mode 100644 index 000000000..bfb2c74ed --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_a/sti_completing_sequence_a_step_3.hpp @@ -0,0 +1,56 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace completing_sequence_a +{ +// STATE DECLARATION +struct StiCompletingSequenceAStep3 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiCompletingSequenceAStep4, TIMEOUT>, + Transition, StiCompletingSequenceAStep2, PREVIOUS>, + Transition, StiCompletingSequenceAStep4, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_a/sti_completing_sequence_a_step_4.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_a/sti_completing_sequence_a_step_4.hpp new file mode 100644 index 000000000..3f0a00ad3 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_a/sti_completing_sequence_a_step_4.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace completing_sequence_a +{ +// STATE DECLARATION +struct StiCompletingSequenceAStep4 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiCompletingSequenceAStep5, TIMEOUT>, + Transition, StiCompletingSequenceAStep3, PREVIOUS>, + Transition, StiCompletingSequenceAStep5, NEXT> + + //Transition, StartStObserve, RETURN>, + // Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_a/sti_completing_sequence_a_step_5.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_a/sti_completing_sequence_a_step_5.hpp new file mode 100644 index 000000000..bee4f97e9 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_a/sti_completing_sequence_a_step_5.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace completing_sequence_a +{ +// STATE DECLARATION +struct StiCompletingSequenceAStep5 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiCompletingSequenceAStep6, TIMEOUT>, + Transition, StiCompletingSequenceAStep4, PREVIOUS>, + Transition, StiCompletingSequenceAStep6, NEXT> + + //Transition, ExecuteStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_a/sti_completing_sequence_a_step_6.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_a/sti_completing_sequence_a_step_6.hpp new file mode 100644 index 000000000..d4ba32098 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_a/sti_completing_sequence_a_step_6.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace completing_sequence_a +{ +// STATE DECLARATION +struct StiCompletingSequenceAStep6 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiCompletingSequenceAStep7, SUCCESS>, + Transition, StiCompletingSequenceAStep8, PREVIOUS>, + Transition, StiCompletingSequenceAStep7, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_a/sti_completing_sequence_a_step_7.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_a/sti_completing_sequence_a_step_7.hpp new file mode 100644 index 000000000..4e47f2463 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_a/sti_completing_sequence_a_step_7.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace completing_sequence_a +{ +// STATE DECLARATION +struct StiCompletingSequenceAStep7 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiCompletingSequenceAStep8, TIMEOUT>, + Transition, StiCompletingSequenceAStep6, PREVIOUS>, + Transition, StiCompletingSequenceAStep8, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_a/sti_completing_sequence_a_step_8.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_a/sti_completing_sequence_a_step_8.hpp new file mode 100644 index 000000000..0f1de3c5f --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_a/sti_completing_sequence_a_step_8.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace completing_sequence_a +{ +// STATE DECLARATION +struct StiCompletingSequenceAStep8 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiCompletingSequenceAStep9, TIMEOUT>, + Transition, StiCompletingSequenceAStep7, PREVIOUS>, + Transition, StiCompletingSequenceAStep9, NEXT> + + //Transition, ExecuteStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_a/sti_completing_sequence_a_step_9.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_a/sti_completing_sequence_a_step_9.hpp new file mode 100644 index 000000000..429393f68 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_a/sti_completing_sequence_a_step_9.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace completing_sequence_a +{ +// STATE DECLARATION +struct StiCompletingSequenceAStep9 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiCompletingSequenceALoop, TIMEOUT>, + Transition, StiCompletingSequenceAStep8, PREVIOUS>, + Transition, StiCompletingSequenceALoop, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_a_loop.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_a_loop.hpp new file mode 100644 index 000000000..8c91cd04d --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_a_loop.hpp @@ -0,0 +1,52 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ + +// STATE DECLARATION +struct CompletingSequenceALoop : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, SsCompletingSequenceA, CONTINUELOOP> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} + + bool loopWhileCondition() + { + auto & superstate = this->context(); + + RCLCPP_INFO( + getLogger(), "Loop start, current iterations: %d, total iterations: %d", + superstate.ziteration_count, superstate.ztotal_iterations()); + return superstate.ziteration_count++ < superstate.ztotal_iterations(); + } + + void onEntry() + { + RCLCPP_INFO(getLogger(), "LOOP START ON ENTRY"); + checkWhileLoopConditionAndThrowEvent(&CompletingSequenceALoop::loopWhileCondition); + } +}; +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_b/sti_completing_sequence_b_loop.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_b/sti_completing_sequence_b_loop.hpp new file mode 100644 index 000000000..811efd61b --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_b/sti_completing_sequence_b_loop.hpp @@ -0,0 +1,54 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace completing_sequence_b +{ +// STATE DECLARATION +struct StiCompletingSequenceBLoop : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiCompletingSequenceBStep1, CONTINUELOOP> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} + + bool loopWhileCondition() + { + auto & superstate = this->context(); + + RCLCPP_INFO( + getLogger(), "Loop start, current iterations: %d, total iterations: %d", + superstate.iteration_count, superstate.total_iterations()); + return superstate.iteration_count++ < superstate.total_iterations(); + } + + void onEntry() + { + RCLCPP_INFO(getLogger(), "LOOP START ON ENTRY"); + checkWhileLoopConditionAndThrowEvent(&StiCompletingSequenceBLoop::loopWhileCondition); + } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_b/sti_completing_sequence_b_step_1.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_b/sti_completing_sequence_b_step_1.hpp new file mode 100644 index 000000000..3eef589fc --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_b/sti_completing_sequence_b_step_1.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace completing_sequence_b +{ +// STATE DECLARATION +struct StiCompletingSequenceBStep1 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiCompletingSequenceBStep2, TIMEOUT>, + Transition, StiCompletingSequenceBStep2, NEXT>, + Transition, StiCompletingSequenceBLoop, PREVIOUS> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_b/sti_completing_sequence_b_step_2.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_b/sti_completing_sequence_b_step_2.hpp new file mode 100644 index 000000000..c37430db8 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_b/sti_completing_sequence_b_step_2.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace completing_sequence_b +{ +// STATE DECLARATION +struct StiCompletingSequenceBStep2 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiCompletingSequenceBStep3, TIMEOUT>, + Transition, StiCompletingSequenceBStep1, PREVIOUS>, + Transition, StiCompletingSequenceBStep3, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_b/sti_completing_sequence_b_step_3.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_b/sti_completing_sequence_b_step_3.hpp new file mode 100644 index 000000000..2ba8450cf --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_b/sti_completing_sequence_b_step_3.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace completing_sequence_b +{ +// STATE DECLARATION +struct StiCompletingSequenceBStep3 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiCompletingSequenceBStep4, SUCCESS>, + Transition, StiCompletingSequenceBStep5, SUCCESS>, + Transition, StiCompletingSequenceBStep6, SUCCESS> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_b/sti_completing_sequence_b_step_4.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_b/sti_completing_sequence_b_step_4.hpp new file mode 100644 index 000000000..e51943acf --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_b/sti_completing_sequence_b_step_4.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace completing_sequence_b +{ +// STATE DECLARATION +struct StiCompletingSequenceBStep4 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiCompletingSequenceBStep7, TIMEOUT>, + Transition, StiCompletingSequenceBStep3, PREVIOUS>, + Transition, StiCompletingSequenceBStep7, NEXT> + + //ransition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_b/sti_completing_sequence_b_step_5.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_b/sti_completing_sequence_b_step_5.hpp new file mode 100644 index 000000000..f74e101db --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_b/sti_completing_sequence_b_step_5.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace completing_sequence_b +{ +// STATE DECLARATION +struct StiCompletingSequenceBStep5 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiCompletingSequenceBStep7, SUCCESS>, + Transition, StiCompletingSequenceBStep3, PREVIOUS>, + Transition, StiCompletingSequenceBStep7, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_b/sti_completing_sequence_b_step_6.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_b/sti_completing_sequence_b_step_6.hpp new file mode 100644 index 000000000..54b7b9547 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_b/sti_completing_sequence_b_step_6.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace completing_sequence_b +{ +// STATE DECLARATION +struct StiCompletingSequenceBStep6 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiCompletingSequenceBStep7, TIMEOUT>, + Transition, StiCompletingSequenceBStep5, PREVIOUS>, + Transition, StiCompletingSequenceBStep7, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_b/sti_completing_sequence_b_step_7.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_b/sti_completing_sequence_b_step_7.hpp new file mode 100644 index 000000000..04b3d48c7 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_b/sti_completing_sequence_b_step_7.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace completing_sequence_b +{ +// STATE DECLARATION +struct StiCompletingSequenceBStep7 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiCompletingSequenceBStep8, TIMEOUT>, + Transition, StiCompletingSequenceBStep6, PREVIOUS>, + Transition, StiCompletingSequenceBStep8, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_b/sti_completing_sequence_b_step_8.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_b/sti_completing_sequence_b_step_8.hpp new file mode 100644 index 000000000..1443b16ba --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_b/sti_completing_sequence_b_step_8.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace completing_sequence_b +{ +// STATE DECLARATION +struct StiCompletingSequenceBStep8 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiCompletingSequenceBStep9, TIMEOUT>, + Transition, StiCompletingSequenceBStep7, PREVIOUS>, + Transition, StiCompletingSequenceBStep9, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_b/sti_completing_sequence_b_step_9.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_b/sti_completing_sequence_b_step_9.hpp new file mode 100644 index 000000000..fbc46b1bf --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_b/sti_completing_sequence_b_step_9.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace completing_sequence_b +{ +// STATE DECLARATION +struct StiCompletingSequenceBStep9 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiCompletingSequenceBLoop, TIMEOUT>, + Transition, StiCompletingSequenceBStep8, PREVIOUS>, + Transition, StiCompletingSequenceBLoop, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_b_loop.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_b_loop.hpp new file mode 100644 index 000000000..25316986e --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_sequence_b_loop.hpp @@ -0,0 +1,52 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ + +// STATE DECLARATION +struct CompletingSequenceBLoop : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, SsCompletingSequenceB, CONTINUELOOP> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} + + bool loopWhileCondition() + { + auto & superstate = this->context(); + + RCLCPP_INFO( + getLogger(), "Loop start, current iterations: %d, total iterations: %d", + superstate.yiteration_count, superstate.ytotal_iterations()); + return superstate.yiteration_count++ < superstate.ytotal_iterations(); + } + + void onEntry() + { + RCLCPP_INFO(getLogger(), "LOOP START ON ENTRY"); + checkWhileLoopConditionAndThrowEvent(&CompletingSequenceBLoop::loopWhileCondition); + } +}; +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_st_observe.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_st_observe.hpp new file mode 100644 index 000000000..192a37072 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/completing_st_observe.hpp @@ -0,0 +1,53 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +// STATE DECLARATION +struct CompletingStObserve : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct completing_sequence_a : SUCCESS{}; + struct completing_sequence_b : SUCCESS{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, CompletingSequenceBLoop, SUCCESS>, + // Transition, SsExecuteSequenceA>, + // Keyboard events + // Transition, MsStarting, SUCCESS>, + Transition, CompletingSequenceALoop, SUCCESS>, + // Transition, SsExecuteSequenceA, execute_sequence_a>, + Transition, CompletingSequenceBLoop, SUCCESS> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(10); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_a/sti_execute_sequence_a_loop.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_a/sti_execute_sequence_a_loop.hpp new file mode 100644 index 000000000..e8ba2a7a7 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_a/sti_execute_sequence_a_loop.hpp @@ -0,0 +1,54 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace execute_sequence_a +{ +// STATE DECLARATION +struct StiExecuteSequenceALoop : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiExecuteSequenceAStep1, CONTINUELOOP> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} + + bool loopWhileCondition() + { + auto & superstate = this->context(); + + RCLCPP_INFO( + getLogger(), "Loop start, current iterations: %d, total iterations: %d", + superstate.iteration_count, superstate.total_iterations()); + return superstate.iteration_count++ < superstate.total_iterations(); + } + + void onEntry() + { + RCLCPP_INFO(getLogger(), "LOOP START ON ENTRY"); + checkWhileLoopConditionAndThrowEvent(&StiExecuteSequenceALoop::loopWhileCondition); + } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_a/sti_execute_sequence_a_step_1.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_a/sti_execute_sequence_a_step_1.hpp new file mode 100644 index 000000000..035adfc3e --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_a/sti_execute_sequence_a_step_1.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace execute_sequence_a +{ +// STATE DECLARATION +struct StiExecuteSequenceAStep1 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiExecuteSequenceAStep2, TIMEOUT>, + Transition, StiExecuteSequenceAStep2, NEXT>, + Transition, StiExecuteSequenceALoop, PREVIOUS> + + // Transition, ExecuteStObserve, RETURN>, + // Transition, MsRecovery1, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_a/sti_execute_sequence_a_step_2.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_a/sti_execute_sequence_a_step_2.hpp new file mode 100644 index 000000000..540e91937 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_a/sti_execute_sequence_a_step_2.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace execute_sequence_a +{ +// STATE DECLARATION +struct StiExecuteSequenceAStep2 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiExecuteSequenceAStep3, TIMEOUT>, + Transition, StiExecuteSequenceAStep1, PREVIOUS>, + Transition, StiExecuteSequenceAStep3, NEXT> + + // Transition, ExecuteStObserve, RETURN>, + // Transition, MsRecovery1, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_a/sti_execute_sequence_a_step_3.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_a/sti_execute_sequence_a_step_3.hpp new file mode 100644 index 000000000..b55a3f612 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_a/sti_execute_sequence_a_step_3.hpp @@ -0,0 +1,56 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace execute_sequence_a +{ +// STATE DECLARATION +struct StiExecuteSequenceAStep3 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiExecuteSequenceAStep4, TIMEOUT>, + Transition, StiExecuteSequenceAStep2, PREVIOUS>, + Transition, StiExecuteSequenceAStep4, NEXT> + + // Transition, ExecuteStObserve, RETURN>, + // Transition, MsRecovery1, ABORT> + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_a/sti_execute_sequence_a_step_4.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_a/sti_execute_sequence_a_step_4.hpp new file mode 100644 index 000000000..b190b1312 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_a/sti_execute_sequence_a_step_4.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace execute_sequence_a +{ +// STATE DECLARATION +struct StiExecuteSequenceAStep4 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiExecuteSequenceAStep5, TIMEOUT>, + Transition, StiExecuteSequenceAStep3, PREVIOUS>, + Transition, StiExecuteSequenceAStep5, NEXT> + + // Transition, ExecuteStObserve, RETURN>, + // Transition, MsRecovery1, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_a/sti_execute_sequence_a_step_5.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_a/sti_execute_sequence_a_step_5.hpp new file mode 100644 index 000000000..f491333fc --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_a/sti_execute_sequence_a_step_5.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace execute_sequence_a +{ +// STATE DECLARATION +struct StiExecuteSequenceAStep5 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiExecuteSequenceAStep6, TIMEOUT>, + Transition, StiExecuteSequenceAStep4, PREVIOUS>, + Transition, StiExecuteSequenceAStep6, NEXT> + + // Transition, ExecuteStObserve, RETURN>, + // Transition, MsRecovery1, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_a/sti_execute_sequence_a_step_6.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_a/sti_execute_sequence_a_step_6.hpp new file mode 100644 index 000000000..bded19979 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_a/sti_execute_sequence_a_step_6.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace execute_sequence_a +{ +// STATE DECLARATION +struct StiExecuteSequenceAStep6 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiExecuteSequenceAStep7, TIMEOUT>, + Transition, StiExecuteSequenceAStep5, PREVIOUS>, + Transition, StiExecuteSequenceAStep7, NEXT> + + // Transition, ExecuteStObserve, RETURN>, + // Transition, MsRecovery1, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_a/sti_execute_sequence_a_step_7.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_a/sti_execute_sequence_a_step_7.hpp new file mode 100644 index 000000000..f80f4b326 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_a/sti_execute_sequence_a_step_7.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace execute_sequence_a +{ +// STATE DECLARATION +struct StiExecuteSequenceAStep7 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiExecuteSequenceAStep8, TIMEOUT>, + Transition, StiExecuteSequenceAStep6, PREVIOUS>, + Transition, StiExecuteSequenceAStep8, NEXT> + + // Transition, ExecuteStObserve, RETURN>, + // Transition, MsRecovery1, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_a/sti_execute_sequence_a_step_8.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_a/sti_execute_sequence_a_step_8.hpp new file mode 100644 index 000000000..80f516f45 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_a/sti_execute_sequence_a_step_8.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace execute_sequence_a +{ +// STATE DECLARATION +struct StiExecuteSequenceAStep8 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiExecuteSequenceAStep9, TIMEOUT>, + Transition, StiExecuteSequenceAStep7, PREVIOUS>, + Transition, StiExecuteSequenceAStep9, NEXT> + + // Transition, ExecuteStObserve, RETURN>, + // Transition, MsRecovery1, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_a/sti_execute_sequence_a_step_9.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_a/sti_execute_sequence_a_step_9.hpp new file mode 100644 index 000000000..f6af2f426 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_a/sti_execute_sequence_a_step_9.hpp @@ -0,0 +1,58 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace execute_sequence_a +{ +// STATE DECLARATION +struct StiExecuteSequenceAStep9 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiExecuteSequenceALoop, TIMEOUT>, + Transition, StiExecuteSequenceAStep8, PREVIOUS>, + Transition, StiExecuteSequenceALoop, NEXT> + + // Transition, MsStarting, SUCCESS>, + // Transition, ExecuteStObserve, RETURN>, + // Transition, MsRecovery1, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_a_loop.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_a_loop.hpp new file mode 100644 index 000000000..2e0744bef --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_a_loop.hpp @@ -0,0 +1,52 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ + +// STATE DECLARATION +struct ExecuteSequenceALoop : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, SsExecuteSequenceA, CONTINUELOOP> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} + + bool loopWhileCondition() + { + auto & superstate = this->context(); + + RCLCPP_INFO( + getLogger(), "Loop start, current iterations: %d, total iterations: %d", + superstate.ziteration_count, superstate.ztotal_iterations()); + return superstate.ziteration_count++ < superstate.ztotal_iterations(); + } + + void onEntry() + { + RCLCPP_INFO(getLogger(), "LOOP START ON ENTRY"); + checkWhileLoopConditionAndThrowEvent(&ExecuteSequenceALoop::loopWhileCondition); + } +}; +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_b/sti_execute_sequence_b_loop.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_b/sti_execute_sequence_b_loop.hpp new file mode 100644 index 000000000..c3f0653f7 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_b/sti_execute_sequence_b_loop.hpp @@ -0,0 +1,54 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace execute_sequence_b +{ +// STATE DECLARATION +struct StiExecuteSequenceBLoop : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiExecuteSequenceBStep1, CONTINUELOOP> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} + + bool loopWhileCondition() + { + auto & superstate = this->context(); + + RCLCPP_INFO( + getLogger(), "Loop start, current iterations: %d, total iterations: %d", + superstate.iteration_count, superstate.total_iterations()); + return superstate.iteration_count++ < superstate.total_iterations(); + } + + void onEntry() + { + RCLCPP_INFO(getLogger(), "LOOP START ON ENTRY"); + checkWhileLoopConditionAndThrowEvent(&StiExecuteSequenceBLoop::loopWhileCondition); + } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_b/sti_execute_sequence_b_step_1.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_b/sti_execute_sequence_b_step_1.hpp new file mode 100644 index 000000000..de5405779 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_b/sti_execute_sequence_b_step_1.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace execute_sequence_b +{ +// STATE DECLARATION +struct StiExecuteSequenceBStep1 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiExecuteSequenceBStep2, TIMEOUT>, + Transition, StiExecuteSequenceBStep2, NEXT>, + Transition, StiExecuteSequenceBLoop, PREVIOUS> + + // Transition, ExecuteStObserve, RETURN>, + // Transition, MsRecovery1, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_b/sti_execute_sequence_b_step_2.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_b/sti_execute_sequence_b_step_2.hpp new file mode 100644 index 000000000..01b46d357 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_b/sti_execute_sequence_b_step_2.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace execute_sequence_b +{ +// STATE DECLARATION +struct StiExecuteSequenceBStep2 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiExecuteSequenceBStep3, TIMEOUT>, + Transition, StiExecuteSequenceBStep1, PREVIOUS>, + Transition, StiExecuteSequenceBStep3, NEXT> + + // Transition, ExecuteStObserve, RETURN>, + // Transition, MsRecovery1, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_b/sti_execute_sequence_b_step_3.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_b/sti_execute_sequence_b_step_3.hpp new file mode 100644 index 000000000..b5330b234 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_b/sti_execute_sequence_b_step_3.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace execute_sequence_b +{ +// STATE DECLARATION +struct StiExecuteSequenceBStep3 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiExecuteSequenceBStep4, TIMEOUT>, + Transition, StiExecuteSequenceBStep2, PREVIOUS>, + Transition, StiExecuteSequenceBStep4, NEXT> + + // Transition, ExecuteStObserve, RETURN>, + // Transition, MsRecovery1, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_b/sti_execute_sequence_b_step_4.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_b/sti_execute_sequence_b_step_4.hpp new file mode 100644 index 000000000..13652b839 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_b/sti_execute_sequence_b_step_4.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace execute_sequence_b +{ +// STATE DECLARATION +struct StiExecuteSequenceBStep4 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiExecuteSequenceBStep5, TIMEOUT>, + Transition, StiExecuteSequenceBStep3, PREVIOUS>, + Transition, StiExecuteSequenceBStep5, NEXT> + + // Transition, ExecuteStObserve, RETURN>, + // Transition, MsRecovery1, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_b/sti_execute_sequence_b_step_5.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_b/sti_execute_sequence_b_step_5.hpp new file mode 100644 index 000000000..5ac7f31db --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_b/sti_execute_sequence_b_step_5.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace execute_sequence_b +{ +// STATE DECLARATION +struct StiExecuteSequenceBStep5 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiExecuteSequenceBStep6, TIMEOUT>, + Transition, StiExecuteSequenceBStep4, PREVIOUS>, + Transition, StiExecuteSequenceBStep6, NEXT> + + // Transition, ExecuteStObserve, RETURN>, + // Transition, MsRecovery1, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_b/sti_execute_sequence_b_step_6.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_b/sti_execute_sequence_b_step_6.hpp new file mode 100644 index 000000000..1ebd53b84 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_b/sti_execute_sequence_b_step_6.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace execute_sequence_b +{ +// STATE DECLARATION +struct StiExecuteSequenceBStep6 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiExecuteSequenceBStep7, TIMEOUT>, + Transition, StiExecuteSequenceBStep5, PREVIOUS>, + Transition, StiExecuteSequenceBStep7, NEXT> + + // Transition, ExecuteStObserve, RETURN>, + // Transition, MsRecovery1, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_b/sti_execute_sequence_b_step_7.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_b/sti_execute_sequence_b_step_7.hpp new file mode 100644 index 000000000..c268968ef --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_b/sti_execute_sequence_b_step_7.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace execute_sequence_b +{ +// STATE DECLARATION +struct StiExecuteSequenceBStep7 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiExecuteSequenceBStep8, SUCCESS>, + Transition, StiExecuteSequenceBStep6, PREVIOUS>, + Transition, StiExecuteSequenceBStep9, NEXT> + + // Transition, ExecuteStObserve, RETURN>, + // Transition, MsRecovery1, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_b/sti_execute_sequence_b_step_8.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_b/sti_execute_sequence_b_step_8.hpp new file mode 100644 index 000000000..d1bdf94c1 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_b/sti_execute_sequence_b_step_8.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace execute_sequence_b +{ +// STATE DECLARATION +struct StiExecuteSequenceBStep8 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiExecuteSequenceBStep9, TIMEOUT>, + Transition, StiExecuteSequenceBStep7, PREVIOUS>, + Transition, StiExecuteSequenceBStep9, NEXT> + + // Transition, ExecuteStObserve, RETURN>, + // Transition, MsRecovery1, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_b/sti_execute_sequence_b_step_9.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_b/sti_execute_sequence_b_step_9.hpp new file mode 100644 index 000000000..6eba1579a --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_b/sti_execute_sequence_b_step_9.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace execute_sequence_b +{ +// STATE DECLARATION +struct StiExecuteSequenceBStep9 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiExecuteSequenceBLoop, TIMEOUT>, + Transition, StiExecuteSequenceBStep8, PREVIOUS>, + Transition, StiExecuteSequenceBLoop, NEXT> + + // Transition, ExecuteStObserve, RETURN>, + // Transition, MsRecovery1, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_b_loop.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_b_loop.hpp new file mode 100644 index 000000000..d998291f3 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_sequence_b_loop.hpp @@ -0,0 +1,52 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ + +// STATE DECLARATION +struct ExecuteSequenceBLoop : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, SsExecuteSequenceB, CONTINUELOOP> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} + + bool loopWhileCondition() + { + auto & superstate = this->context(); + + RCLCPP_INFO( + getLogger(), "Loop start, current iterations: %d, total iterations: %d", + superstate.yiteration_count, superstate.ytotal_iterations()); + return superstate.yiteration_count++ < superstate.ytotal_iterations(); + } + + void onEntry() + { + RCLCPP_INFO(getLogger(), "LOOP START ON ENTRY"); + checkWhileLoopConditionAndThrowEvent(&ExecuteSequenceBLoop::loopWhileCondition); + } +}; +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_st_observe.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_st_observe.hpp new file mode 100644 index 000000000..ed1ea426f --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/execute_st_observe.hpp @@ -0,0 +1,50 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +// STATE DECLARATION +struct ExecuteStObserve : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct execute_sequence_a : SUCCESS{}; + struct execute_sequence_b : SUCCESS{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, ExecuteSequenceALoop, SUCCESS>, + Transition, ExecuteSequenceBLoop, SUCCESS>, + + Transition, ExecuteSequenceBLoop, SUCCESS> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(10); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_a/sti_holding_sequence_a_loop.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_a/sti_holding_sequence_a_loop.hpp new file mode 100644 index 000000000..a40ec8aa2 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_a/sti_holding_sequence_a_loop.hpp @@ -0,0 +1,54 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace holding_sequence_a +{ +// STATE DECLARATION +struct StiHoldingSequenceALoop : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiHoldingSequenceAStep1, CONTINUELOOP> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} + + bool loopWhileCondition() + { + auto & superstate = this->context(); + + RCLCPP_INFO( + getLogger(), "Loop start, current iterations: %d, total iterations: %d", + superstate.iteration_count, superstate.total_iterations()); + return superstate.iteration_count++ < superstate.total_iterations(); + } + + void onEntry() + { + RCLCPP_INFO(getLogger(), "LOOP START ON ENTRY"); + checkWhileLoopConditionAndThrowEvent(&StiHoldingSequenceALoop::loopWhileCondition); + } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_a/sti_holding_sequence_a_step_1.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_a/sti_holding_sequence_a_step_1.hpp new file mode 100644 index 000000000..02a927e69 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_a/sti_holding_sequence_a_step_1.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace holding_sequence_a +{ +// STATE DECLARATION +struct StiHoldingSequenceAStep1 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiHoldingSequenceAStep2, TIMEOUT>, + Transition, StiHoldingSequenceAStep2, NEXT> + //Transition, StiStartSequenceALoop, PREVIOUS>, + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_a/sti_holding_sequence_a_step_2.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_a/sti_holding_sequence_a_step_2.hpp new file mode 100644 index 000000000..fd16fcfc1 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_a/sti_holding_sequence_a_step_2.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace holding_sequence_a +{ +// STATE DECLARATION +struct StiHoldingSequenceAStep2 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiHoldingSequenceAStep3, TIMEOUT>, + Transition, StiHoldingSequenceAStep1, PREVIOUS>, + Transition, StiHoldingSequenceAStep3, NEXT> + + //Transition, ExecuteStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_a/sti_holding_sequence_a_step_3.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_a/sti_holding_sequence_a_step_3.hpp new file mode 100644 index 000000000..6dac2b5a1 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_a/sti_holding_sequence_a_step_3.hpp @@ -0,0 +1,56 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace holding_sequence_a +{ +// STATE DECLARATION +struct StiHoldingSequenceAStep3 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiHoldingSequenceAStep5, SUCCESS>, + Transition, StiHoldingSequenceAStep2, PREVIOUS>, + Transition, StiHoldingSequenceAStep4, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_a/sti_holding_sequence_a_step_4.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_a/sti_holding_sequence_a_step_4.hpp new file mode 100644 index 000000000..89f29b36a --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_a/sti_holding_sequence_a_step_4.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace holding_sequence_a +{ +// STATE DECLARATION +struct StiHoldingSequenceAStep4 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiHoldingSequenceAStep5, TIMEOUT>, + Transition, StiHoldingSequenceAStep3, PREVIOUS>, + Transition, StiHoldingSequenceAStep5, NEXT> + + //Transition, StartStObserve, RETURN>, + // Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_a/sti_holding_sequence_a_step_5.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_a/sti_holding_sequence_a_step_5.hpp new file mode 100644 index 000000000..47072e2b8 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_a/sti_holding_sequence_a_step_5.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace holding_sequence_a +{ +// STATE DECLARATION +struct StiHoldingSequenceAStep5 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiHoldingSequenceAStep6, TIMEOUT>, + Transition, StiHoldingSequenceAStep4, PREVIOUS>, + Transition, StiHoldingSequenceAStep6, NEXT> + + //Transition, ExecuteStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_a/sti_holding_sequence_a_step_6.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_a/sti_holding_sequence_a_step_6.hpp new file mode 100644 index 000000000..9963fe5cb --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_a/sti_holding_sequence_a_step_6.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace holding_sequence_a +{ +// STATE DECLARATION +struct StiHoldingSequenceAStep6 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiHoldingSequenceAStep7, TIMEOUT>, + Transition, StiHoldingSequenceAStep5, PREVIOUS>, + Transition, StiHoldingSequenceAStep7, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_a/sti_holding_sequence_a_step_7.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_a/sti_holding_sequence_a_step_7.hpp new file mode 100644 index 000000000..0b13e68dd --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_a/sti_holding_sequence_a_step_7.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace holding_sequence_a +{ +// STATE DECLARATION +struct StiHoldingSequenceAStep7 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiHoldingSequenceAStep9, SUCCESS>, + Transition, StiHoldingSequenceAStep6, PREVIOUS>, + Transition, StiHoldingSequenceAStep8, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_a/sti_holding_sequence_a_step_8.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_a/sti_holding_sequence_a_step_8.hpp new file mode 100644 index 000000000..9cdcf85db --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_a/sti_holding_sequence_a_step_8.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace holding_sequence_a +{ +// STATE DECLARATION +struct StiHoldingSequenceAStep8 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiHoldingSequenceAStep9, TIMEOUT>, + Transition, StiHoldingSequenceAStep7, PREVIOUS>, + Transition, StiHoldingSequenceAStep9, NEXT> + + //Transition, ExecuteStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_a/sti_holding_sequence_a_step_9.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_a/sti_holding_sequence_a_step_9.hpp new file mode 100644 index 000000000..467325ec3 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_a/sti_holding_sequence_a_step_9.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace holding_sequence_a +{ +// STATE DECLARATION +struct StiHoldingSequenceAStep9 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiHoldingSequenceALoop, TIMEOUT>, + Transition, StiHoldingSequenceAStep8, PREVIOUS>, + Transition, StiHoldingSequenceALoop, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_a_loop.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_a_loop.hpp new file mode 100644 index 000000000..786f8ebb4 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_a_loop.hpp @@ -0,0 +1,52 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ + +// STATE DECLARATION +struct HoldingSequenceALoop : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, SsHoldingSequenceA, CONTINUELOOP> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} + + bool loopWhileCondition() + { + auto & superstate = this->context(); + + RCLCPP_INFO( + getLogger(), "Loop start, current iterations: %d, total iterations: %d", + superstate.ziteration_count, superstate.ztotal_iterations()); + return superstate.ziteration_count++ < superstate.ztotal_iterations(); + } + + void onEntry() + { + RCLCPP_INFO(getLogger(), "LOOP START ON ENTRY"); + checkWhileLoopConditionAndThrowEvent(&HoldingSequenceALoop::loopWhileCondition); + } +}; +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_b/sti_holding_sequence_b_loop.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_b/sti_holding_sequence_b_loop.hpp new file mode 100644 index 000000000..c8d9da003 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_b/sti_holding_sequence_b_loop.hpp @@ -0,0 +1,54 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace holding_sequence_b +{ +// STATE DECLARATION +struct StiHoldingSequenceBLoop : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiHoldingSequenceBStep1, CONTINUELOOP> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} + + bool loopWhileCondition() + { + auto & superstate = this->context(); + + RCLCPP_INFO( + getLogger(), "Loop start, current iterations: %d, total iterations: %d", + superstate.iteration_count, superstate.total_iterations()); + return superstate.iteration_count++ < superstate.total_iterations(); + } + + void onEntry() + { + RCLCPP_INFO(getLogger(), "LOOP START ON ENTRY"); + checkWhileLoopConditionAndThrowEvent(&StiHoldingSequenceBLoop::loopWhileCondition); + } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_b/sti_holding_sequence_b_step_1.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_b/sti_holding_sequence_b_step_1.hpp new file mode 100644 index 000000000..d76004049 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_b/sti_holding_sequence_b_step_1.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace holding_sequence_b +{ +// STATE DECLARATION +struct StiHoldingSequenceBStep1 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiHoldingSequenceBStep2, TIMEOUT>, + Transition, StiHoldingSequenceBStep2, NEXT>, + Transition, StiHoldingSequenceBLoop, PREVIOUS> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_b/sti_holding_sequence_b_step_2.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_b/sti_holding_sequence_b_step_2.hpp new file mode 100644 index 000000000..a235f39cd --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_b/sti_holding_sequence_b_step_2.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace holding_sequence_b +{ +// STATE DECLARATION +struct StiHoldingSequenceBStep2 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiHoldingSequenceBStep3, TIMEOUT>, + Transition, StiHoldingSequenceBStep1, PREVIOUS>, + Transition, StiHoldingSequenceBStep3, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_b/sti_holding_sequence_b_step_3.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_b/sti_holding_sequence_b_step_3.hpp new file mode 100644 index 000000000..d4df3ef31 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_b/sti_holding_sequence_b_step_3.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace holding_sequence_b +{ +// STATE DECLARATION +struct StiHoldingSequenceBStep3 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiHoldingSequenceBStep4, TIMEOUT>, + Transition, StiHoldingSequenceBStep2, PREVIOUS>, + Transition, StiHoldingSequenceBStep4, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_b/sti_holding_sequence_b_step_4.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_b/sti_holding_sequence_b_step_4.hpp new file mode 100644 index 000000000..7d09a1fe7 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_b/sti_holding_sequence_b_step_4.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace holding_sequence_b +{ +// STATE DECLARATION +struct StiHoldingSequenceBStep4 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiHoldingSequenceBStep5, TIMEOUT>, + Transition, StiHoldingSequenceBStep3, PREVIOUS>, + Transition, StiHoldingSequenceBStep5, NEXT> + + //ransition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_b/sti_holding_sequence_b_step_5.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_b/sti_holding_sequence_b_step_5.hpp new file mode 100644 index 000000000..89d8dd195 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_b/sti_holding_sequence_b_step_5.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace holding_sequence_b +{ +// STATE DECLARATION +struct StiHoldingSequenceBStep5 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiHoldingSequenceBStep6, TIMEOUT>, + Transition, StiHoldingSequenceBStep4, PREVIOUS>, + Transition, StiHoldingSequenceBStep6, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_b/sti_holding_sequence_b_step_6.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_b/sti_holding_sequence_b_step_6.hpp new file mode 100644 index 000000000..154e12f68 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_b/sti_holding_sequence_b_step_6.hpp @@ -0,0 +1,59 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace holding_sequence_b +{ + using cl_keyboard::CbDefaultKeyboardBehavior; + +// STATE DECLARATION +struct StiHoldingSequenceBStep6 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiHoldingSequenceBStep8, SUCCESS>, + Transition, StiHoldingSequenceBStep5, PREVIOUS>, + Transition, StiHoldingSequenceBStep7, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_b/sti_holding_sequence_b_step_7.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_b/sti_holding_sequence_b_step_7.hpp new file mode 100644 index 000000000..b7139e69f --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_b/sti_holding_sequence_b_step_7.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace holding_sequence_b +{ +// STATE DECLARATION +struct StiHoldingSequenceBStep7 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiHoldingSequenceBStep8, TIMEOUT>, + Transition, StiHoldingSequenceBStep6, PREVIOUS>, + Transition, StiHoldingSequenceBStep8, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_b/sti_holding_sequence_b_step_8.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_b/sti_holding_sequence_b_step_8.hpp new file mode 100644 index 000000000..53755a706 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_b/sti_holding_sequence_b_step_8.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace holding_sequence_b +{ +// STATE DECLARATION +struct StiHoldingSequenceBStep8 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiHoldingSequenceBStep9, TIMEOUT>, + Transition, StiHoldingSequenceBStep7, PREVIOUS>, + Transition, StiHoldingSequenceBStep9, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_b/sti_holding_sequence_b_step_9.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_b/sti_holding_sequence_b_step_9.hpp new file mode 100644 index 000000000..89dc36f65 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_b/sti_holding_sequence_b_step_9.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace holding_sequence_b +{ +// STATE DECLARATION +struct StiHoldingSequenceBStep9 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiHoldingSequenceBLoop, TIMEOUT>, + Transition, StiHoldingSequenceBStep8, PREVIOUS>, + Transition, StiHoldingSequenceBLoop, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_b_loop.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_b_loop.hpp new file mode 100644 index 000000000..16c330357 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_sequence_b_loop.hpp @@ -0,0 +1,52 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ + +// STATE DECLARATION +struct HoldingSequenceBLoop : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, SsHoldingSequenceB, CONTINUELOOP> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} + + bool loopWhileCondition() + { + auto & superstate = this->context(); + + RCLCPP_INFO( + getLogger(), "Loop start, current iterations: %d, total iterations: %d", + superstate.yiteration_count, superstate.ytotal_iterations()); + return superstate.yiteration_count++ < superstate.ytotal_iterations(); + } + + void onEntry() + { + RCLCPP_INFO(getLogger(), "LOOP START ON ENTRY"); + checkWhileLoopConditionAndThrowEvent(&HoldingSequenceBLoop::loopWhileCondition); + } +}; +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_st_observe.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_st_observe.hpp new file mode 100644 index 000000000..ab8f0834e --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/holding_st_observe.hpp @@ -0,0 +1,53 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +// STATE DECLARATION +struct HoldingStObserve : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct holding_sequence_a : SUCCESS{}; + struct holding_sequence_b : SUCCESS{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, HoldingSequenceBLoop, SUCCESS>, + // Transition, SsExecuteSequenceA>, + // Keyboard events + // Transition, MsStarting, SUCCESS>, + Transition, HoldingSequenceALoop, SUCCESS>, + // Transition, SsExecuteSequenceA, execute_sequence_a>, + Transition, HoldingSequenceBLoop, SUCCESS> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(10); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/ms_recovery_1/st_recovery_analyze_1.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/ms_recovery_1/st_recovery_analyze_1.hpp new file mode 100644 index 000000000..b706d8835 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/ms_recovery_1/st_recovery_analyze_1.hpp @@ -0,0 +1,53 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +// STATE DECLARATION +struct StRecoveryAnalyze1 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StRecoveryBifurcate1, TIMEOUT>, + // Transition, SsExecuteSequenceA>, + // Keyboard events + Transition, StRecoveryBifurcate1, SUCCESS> + // Transition, SsExecuteSequenceB, BUILD>, + // Transition, SsPCCycle, ATTACK> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(50); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/ms_recovery_1/st_recovery_bifurcate_1.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/ms_recovery_1/st_recovery_bifurcate_1.hpp new file mode 100644 index 000000000..9aa30d76c --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/ms_recovery_1/st_recovery_bifurcate_1.hpp @@ -0,0 +1,53 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +// STATE DECLARATION +struct StRecoveryBifurcate1 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StRecoveryCalculate1, SUCCESS> + // Transition, SsExecuteSequenceA>, + // Keyboard events + // Transition, SsExecuteSequenceA, MOVE>, + // Transition, SsExecuteSequenceB, BUILD>, + // Transition, SsPCCycle, ATTACK> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(50); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/ms_recovery_1/st_recovery_calculate_1.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/ms_recovery_1/st_recovery_calculate_1.hpp new file mode 100644 index 000000000..af54a251a --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/ms_recovery_1/st_recovery_calculate_1.hpp @@ -0,0 +1,53 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +// STATE DECLARATION +struct StRecoveryCalculate1 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StRecoveryDeliberate1, SUCCESS> + // Transition, SsExecuteSequenceA>, + // Keyboard events + // Transition, SsExecuteSequenceA, MOVE>, + // Transition, SsExecuteSequenceB, BUILD>, + // Transition, SsPCCycle, ATTACK> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(50); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/ms_recovery_1/st_recovery_deliberate_1.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/ms_recovery_1/st_recovery_deliberate_1.hpp new file mode 100644 index 000000000..cc8ccc3d1 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/ms_recovery_1/st_recovery_deliberate_1.hpp @@ -0,0 +1,49 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +// STATE DECLARATION +struct StRecoveryDeliberate1 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StRecoveryEvaluate1, NEXT>, + Transition, smacc2::deep_history, SUCCESS> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(50); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/ms_recovery_1/st_recovery_evaluate_1.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/ms_recovery_1/st_recovery_evaluate_1.hpp new file mode 100644 index 000000000..5f03ea688 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/ms_recovery_1/st_recovery_evaluate_1.hpp @@ -0,0 +1,53 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +// STATE DECLARATION +struct StRecoveryEvaluate1 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StRecoveryGenerate1, SUCCESS> + // Transition, SsExecuteSequenceA>, + // Keyboard events + // Transition, SsExecuteSequenceA, MOVE>, + // Transition, SsExecuteSequenceB, BUILD>, + // Transition, SsPCCycle, ATTACK> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(50); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/ms_recovery_1/st_recovery_generate_1.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/ms_recovery_1/st_recovery_generate_1.hpp new file mode 100644 index 000000000..8c1e007b8 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/ms_recovery_1/st_recovery_generate_1.hpp @@ -0,0 +1,53 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +// STATE DECLARATION +struct StRecoveryGenerate1 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StRecoveryInnervate1, SUCCESS> + // Transition, SsExecuteSequenceA>, + // Keyboard events + // Transition, SsExecuteSequenceA, MOVE>, + // Transition, SsExecuteSequenceB, BUILD>, + // Transition, SsPCCycle, ATTACK> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(50); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/ms_recovery_1/st_recovery_innervate_1.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/ms_recovery_1/st_recovery_innervate_1.hpp new file mode 100644 index 000000000..4cbc7ba3f --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/ms_recovery_1/st_recovery_innervate_1.hpp @@ -0,0 +1,48 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +// STATE DECLARATION +struct StRecoveryInnervate1 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, smacc2::deep_history, SUCCESS> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(50); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/ms_recovery_2/st_recovery_analyze_2.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/ms_recovery_2/st_recovery_analyze_2.hpp new file mode 100644 index 000000000..51f5c5bff --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/ms_recovery_2/st_recovery_analyze_2.hpp @@ -0,0 +1,53 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +// STATE DECLARATION +struct StRecoveryAnalyze2 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StRecoveryBifurcate2, TIMEOUT>, + // Transition, SsExecuteSequenceA>, + // Keyboard events + Transition, StRecoveryBifurcate2, SUCCESS> + // Transition, SsExecuteSequenceB, BUILD>, + // Transition, SsPCCycle, ATTACK> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(50); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/ms_recovery_2/st_recovery_bifurcate_2.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/ms_recovery_2/st_recovery_bifurcate_2.hpp new file mode 100644 index 000000000..f0f87d76f --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/ms_recovery_2/st_recovery_bifurcate_2.hpp @@ -0,0 +1,53 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +// STATE DECLARATION +struct StRecoveryBifurcate2 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StRecoveryCalculate2, SUCCESS> + // Transition, SsExecuteSequenceA>, + // Keyboard events + // Transition, SsExecuteSequenceA, MOVE>, + // Transition, SsExecuteSequenceB, BUILD>, + // Transition, SsPCCycle, ATTACK> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(50); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/ms_recovery_2/st_recovery_calculate_2.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/ms_recovery_2/st_recovery_calculate_2.hpp new file mode 100644 index 000000000..8dcf55751 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/ms_recovery_2/st_recovery_calculate_2.hpp @@ -0,0 +1,53 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +// STATE DECLARATION +struct StRecoveryCalculate2 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + + // TRANSITION TABLE + typedef mpl::list< + + //Transition, StRecoveryDeliberate2, SUCCESS> + // Transition, SsExecuteSequenceA>, + // Keyboard events + // Transition, SsExecuteSequenceA, MOVE>, + // Transition, SsExecuteSequenceB, BUILD>, + // Transition, SsPCCycle, ATTACK> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(50); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/ms_recovery_2/st_recovery_deliberate_2.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/ms_recovery_2/st_recovery_deliberate_2.hpp new file mode 100644 index 000000000..a51e9cffb --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/ms_recovery_2/st_recovery_deliberate_2.hpp @@ -0,0 +1,49 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +// STATE DECLARATION +struct StRecoveryDeliberate2 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StRecoveryEvaluate2, NEXT>, + Transition, smacc2::deep_history, SUCCESS> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(50); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/ms_recovery_2/st_recovery_evaluate_2.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/ms_recovery_2/st_recovery_evaluate_2.hpp new file mode 100644 index 000000000..3ab5b12eb --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/ms_recovery_2/st_recovery_evaluate_2.hpp @@ -0,0 +1,53 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +// STATE DECLARATION +struct StRecoveryEvaluate2 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StRecoveryGenerate2, SUCCESS> + // Transition, SsExecuteSequenceA>, + // Keyboard events + // Transition, SsExecuteSequenceA, MOVE>, + // Transition, SsExecuteSequenceB, BUILD>, + // Transition, SsPCCycle, ATTACK> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(50); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/ms_recovery_2/st_recovery_generate_2.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/ms_recovery_2/st_recovery_generate_2.hpp new file mode 100644 index 000000000..15e029a9f --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/ms_recovery_2/st_recovery_generate_2.hpp @@ -0,0 +1,53 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +// STATE DECLARATION +struct StRecoveryGenerate2 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StRecoveryInnervate2, SUCCESS> + // Transition, SsExecuteSequenceA>, + // Keyboard events + // Transition, SsExecuteSequenceA, MOVE>, + // Transition, SsExecuteSequenceB, BUILD>, + // Transition, SsPCCycle, ATTACK> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(50); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/ms_recovery_2/st_recovery_innervate_2.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/ms_recovery_2/st_recovery_innervate_2.hpp new file mode 100644 index 000000000..5effbed55 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/ms_recovery_2/st_recovery_innervate_2.hpp @@ -0,0 +1,48 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +// STATE DECLARATION +struct StRecoveryInnervate2 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, smacc2::deep_history, SUCCESS> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(50); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/st_acquire_sensors.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/st_acquire_sensors.hpp new file mode 100644 index 000000000..c7c84b626 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/st_acquire_sensors.hpp @@ -0,0 +1,62 @@ +// Copyright 2021 MyName/MyCompany Inc. +// Copyright 2021 RobosoftAI Inc. (template) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + *****************************************************************************************************************/ + +#pragma once + +#include + +namespace sm_multi_panda_sim +{ +// SMACC2 classes +using smacc2::EvStateRequestFinish; +using smacc2::Transition; +using smacc2::default_transition_tags::SUCCESS; +using namespace smacc2; +using namespace cl_move_group_interface; +using smacc2::client_behaviors::CbWaitTopicMessage; +using namespace std::chrono_literals; +using cl_move_group_interface::CbWaitJointState; + +// STATE DECLARATION +struct StAcquireSensors : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef boost::mpl::list< + Transition< + EvCbSuccess, StMoveJoints, SUCCESS> + > reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(); + }; + + void onEntry() + { + rclcpp::sleep_for(5s); + } + + void runtimeConfigure() {} +}; +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/st_move_joints.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/st_move_joints.hpp new file mode 100644 index 000000000..23f9a7a56 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/st_move_joints.hpp @@ -0,0 +1,108 @@ +// Copyright 2021 MyName/MyCompany Inc. +// Copyright 2021 RobosoftAI Inc. (template) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + *****************************************************************************************************************/ + +#pragma once + +namespace sm_multi_panda_sim +{ +// SMACC2 classes +using smacc2::EvStateRequestFinish; +using smacc2::Transition; +using smacc2::default_transition_tags::SUCCESS; +using namespace smacc2; +using namespace cl_move_group_interface; + + +// STATE DECLARATION +struct StMoveJoints : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef boost::mpl::list< + // Transition, StMoveEndEffector, SUCCESS> + + > + reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + // std::map leftJointValues{ + // {"left_panda_joint1", 0.0}, + // {"left_panda_joint2", 0.0}, + // {"left_panda_joint3", 0.0}, + // {"left_panda_joint4", 0.0}, + // {"left_panda_joint5", M_PI/2.0}, + // {"left_panda_joint7", 0.0}, + // {"left_panda_joint6", 0.0}, + // {"right_panda_joint1", 0.0}, + // {"right_panda_joint2", M_PI/2.0}, + // {"right_panda_joint3", 0.0}, + // {"right_panda_joint4", 0.0}, + // {"right_panda_joint5", 0.0}, + // {"right_panda_joint6", 0.0}, + // {"right_panda_joint7", 0.0}, + + // }; + + // configure_orthogonal(leftJointValues); + + geometry_msgs::msg::PoseStamped endeffector1pose; + endeffector1pose.pose.orientation.w = 1; + endeffector1pose.header.frame_id = "left_panda_link8"; + + geometry_msgs::msg::PoseStamped endeffector2pose; + endeffector2pose.pose.orientation.w = 1; + endeffector2pose.header.frame_id = "right_panda_link8"; + + //configure_orthogonal(std::vector{endeffector1pose},std::vector{endeffector1pose.header.frame_id}); + configure_orthogonal(std::vector{endeffector2pose},std::vector{endeffector2pose.header.frame_id}); + +// configure_orthogonal(std::vector{endeffector1pose, endeffector2pose},std::vector{endeffector1pose.header.frame_id, endeffector2pose.header.frame_id}); + + // std::map rightJointValues{ + // {"right_panda_joint1", 0.0}, + // {"right_panda_joint2", 0.0}, + // {"right_panda_joint3", 0.0}, + // {"right_panda_joint4", 0.0}, + // {"right_panda_joint5", 0.0}, + // {"right_panda_joint7", 0.0}, + // {"right_panda_joint6", 0.0}, + // // {"right_panda_leftfinger", 0.0}, + // // {"right_panda_rightfinger", 0.0} + // }; + + // configure_orthogonal(rightJointValues); + }; + + void runtimeConfigure() + { + // ClMoveGroup * moveGroupClientLeft; + // this->requiresClient(moveGroupClientLeft); + // this->getOrthogonal()->getClientBehavior()->scalingFactor_ = 0.1; + + // ClMoveGroup * moveGroupClientRight; + // this->requiresClient(moveGroupClientRight); + // this->getOrthogonal()->getClientBehavior()->scalingFactor_ = 0.1; + } +}; +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_a/sti_start_sequence_a_loop.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_a/sti_start_sequence_a_loop.hpp new file mode 100644 index 000000000..8144c9c92 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_a/sti_start_sequence_a_loop.hpp @@ -0,0 +1,54 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace start_sequence_a +{ +// STATE DECLARATION +struct StiStartSequenceALoop : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiStartSequenceAStep1, CONTINUELOOP> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} + + bool loopWhileCondition() + { + auto & superstate = this->context(); + + RCLCPP_INFO( + getLogger(), "Loop start, current iterations: %d, total iterations: %d", + superstate.iteration_count, superstate.total_iterations()); + return superstate.iteration_count++ < superstate.total_iterations(); + } + + void onEntry() + { + RCLCPP_INFO(getLogger(), "LOOP START ON ENTRY"); + checkWhileLoopConditionAndThrowEvent(&StiStartSequenceALoop::loopWhileCondition); + } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_a/sti_start_sequence_a_step_1.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_a/sti_start_sequence_a_step_1.hpp new file mode 100644 index 000000000..0883af971 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_a/sti_start_sequence_a_step_1.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace start_sequence_a +{ +// STATE DECLARATION +struct StiStartSequenceAStep1 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiStartSequenceAStep2, SUCCESS>, + Transition, StiStartSequenceAStep2, NEXT> + //Transition, StiStartSequenceALoop, PREVIOUS>, + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_a/sti_start_sequence_a_step_2.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_a/sti_start_sequence_a_step_2.hpp new file mode 100644 index 000000000..d869b8cd5 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_a/sti_start_sequence_a_step_2.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace start_sequence_a +{ +// STATE DECLARATION +struct StiStartSequenceAStep2 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiStartSequenceAStep3, TIMEOUT>, + Transition, StiStartSequenceAStep1, PREVIOUS>, + Transition, StiStartSequenceAStep3, NEXT> + + //Transition, ExecuteStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_a/sti_start_sequence_a_step_3.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_a/sti_start_sequence_a_step_3.hpp new file mode 100644 index 000000000..a01155b9f --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_a/sti_start_sequence_a_step_3.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace start_sequence_a +{ +// STATE DECLARATION +struct StiStartSequenceAStep3 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiStartSequenceAStep4, SUCCESS>, + Transition, StiStartSequenceAStep2, PREVIOUS>, + Transition, StiStartSequenceAStep4, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_a/sti_start_sequence_a_step_4.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_a/sti_start_sequence_a_step_4.hpp new file mode 100644 index 000000000..9d9d1f4f5 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_a/sti_start_sequence_a_step_4.hpp @@ -0,0 +1,58 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace start_sequence_a +{ +// STATE DECLARATION +struct StiStartSequenceAStep4 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiStartSequenceAStep5, SUCCESS>, + Transition, StiStartSequenceAStep3, PREVIOUS>, + Transition, StiStartSequenceAStep5, NEXT> + + // Transition, StartStObserve, RETURN>, + // Transition, MsRecovery2, ABORT> + + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_a/sti_start_sequence_a_step_5.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_a/sti_start_sequence_a_step_5.hpp new file mode 100644 index 000000000..3f51edfa0 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_a/sti_start_sequence_a_step_5.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace start_sequence_a +{ +// STATE DECLARATION +struct StiStartSequenceAStep5 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiStartSequenceAStep7, SUCCESS>, + Transition, StiStartSequenceAStep4, PREVIOUS>, + Transition, StiStartSequenceAStep6, NEXT> + + //Transition, ExecuteStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_a/sti_start_sequence_a_step_6.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_a/sti_start_sequence_a_step_6.hpp new file mode 100644 index 000000000..9cef13e97 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_a/sti_start_sequence_a_step_6.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace start_sequence_a +{ +// STATE DECLARATION +struct StiStartSequenceAStep6 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiStartSequenceAStep8, TIMEOUT>, + // Transition, StiStartSequenceAStep52, PREVIOUS>, + Transition, StiStartSequenceAStep8, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_a/sti_start_sequence_a_step_7.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_a/sti_start_sequence_a_step_7.hpp new file mode 100644 index 000000000..d795dc677 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_a/sti_start_sequence_a_step_7.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace start_sequence_a +{ +// STATE DECLARATION +struct StiStartSequenceAStep7 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiStartSequenceAStep8, TIMEOUT>, + // Transition, StiStartSequenceAStep6, PREVIOUS>, + Transition, StiStartSequenceAStep8, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_a/sti_start_sequence_a_step_8.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_a/sti_start_sequence_a_step_8.hpp new file mode 100644 index 000000000..616b98e69 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_a/sti_start_sequence_a_step_8.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace start_sequence_a +{ +// STATE DECLARATION +struct StiStartSequenceAStep8 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiStartSequenceAStep9, TIMEOUT>, + Transition, StiStartSequenceAStep7, PREVIOUS>, + Transition, StiStartSequenceAStep9, NEXT> + + //Transition, ExecuteStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_a/sti_start_sequence_a_step_9.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_a/sti_start_sequence_a_step_9.hpp new file mode 100644 index 000000000..85d7cf4e0 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_a/sti_start_sequence_a_step_9.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace start_sequence_a +{ +// STATE DECLARATION +struct StiStartSequenceAStep9 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiStartSequenceALoop, TIMEOUT>, + Transition, StiStartSequenceAStep8, PREVIOUS>, + Transition, StiStartSequenceALoop, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_a_loop.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_a_loop.hpp new file mode 100644 index 000000000..cd7ce91ed --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_a_loop.hpp @@ -0,0 +1,52 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ + +// STATE DECLARATION +struct StartSequenceALoop : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, SsStartSequenceA, CONTINUELOOP> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} + + bool loopWhileCondition() + { + auto & superstate = this->context(); + + RCLCPP_INFO( + getLogger(), "Loop start, current iterations: %d, total iterations: %d", + superstate.ziteration_count, superstate.ztotal_iterations()); + return superstate.ziteration_count++ < superstate.ztotal_iterations(); + } + + void onEntry() + { + RCLCPP_INFO(getLogger(), "LOOP START ON ENTRY"); + checkWhileLoopConditionAndThrowEvent(&StartSequenceALoop::loopWhileCondition); + } +}; +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_b/sti_start_sequence_b_loop.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_b/sti_start_sequence_b_loop.hpp new file mode 100644 index 000000000..509e56fd0 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_b/sti_start_sequence_b_loop.hpp @@ -0,0 +1,54 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace start_sequence_b +{ +// STATE DECLARATION +struct StiStartSequenceBLoop : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiStartSequenceBStep1, CONTINUELOOP> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} + + bool loopWhileCondition() + { + auto & superstate = this->context(); + + RCLCPP_INFO( + getLogger(), "Loop start, current iterations: %d, total iterations: %d", + superstate.iteration_count, superstate.total_iterations()); + return superstate.iteration_count++ < superstate.total_iterations(); + } + + void onEntry() + { + RCLCPP_INFO(getLogger(), "LOOP START ON ENTRY"); + checkWhileLoopConditionAndThrowEvent(&StiStartSequenceBLoop::loopWhileCondition); + } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_b/sti_start_sequence_b_step_1.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_b/sti_start_sequence_b_step_1.hpp new file mode 100644 index 000000000..097df6849 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_b/sti_start_sequence_b_step_1.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace start_sequence_b +{ +// STATE DECLARATION +struct StiStartSequenceBStep1 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiStartSequenceBStep2, SUCCESS>, + Transition, StiStartSequenceBStep3, NEXT>, + Transition, StiStartSequenceBLoop, PREVIOUS> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_b/sti_start_sequence_b_step_2.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_b/sti_start_sequence_b_step_2.hpp new file mode 100644 index 000000000..c6c05b729 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_b/sti_start_sequence_b_step_2.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace start_sequence_b +{ +// STATE DECLARATION +struct StiStartSequenceBStep2 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiStartSequenceBStep3, TIMEOUT>, + Transition, StiStartSequenceBStep1, PREVIOUS>, + Transition, StiStartSequenceBStep3, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_b/sti_start_sequence_b_step_3.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_b/sti_start_sequence_b_step_3.hpp new file mode 100644 index 000000000..78825784b --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_b/sti_start_sequence_b_step_3.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace start_sequence_b +{ +// STATE DECLARATION +struct StiStartSequenceBStep3 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiStartSequenceBStep4, TIMEOUT>, + Transition, StiStartSequenceBStep2, PREVIOUS>, + Transition, StiStartSequenceBStep4, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_b/sti_start_sequence_b_step_4.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_b/sti_start_sequence_b_step_4.hpp new file mode 100644 index 000000000..2d0825c4a --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_b/sti_start_sequence_b_step_4.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace start_sequence_b +{ +// STATE DECLARATION +struct StiStartSequenceBStep4 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiStartSequenceBStep5, TIMEOUT>, + Transition, StiStartSequenceBStep3, PREVIOUS>, + Transition, StiStartSequenceBStep5, NEXT> + + //ransition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_b/sti_start_sequence_b_step_5.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_b/sti_start_sequence_b_step_5.hpp new file mode 100644 index 000000000..f35166acc --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_b/sti_start_sequence_b_step_5.hpp @@ -0,0 +1,58 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace start_sequence_b +{ +// STATE DECLARATION +struct StiStartSequenceBStep5 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiStartSequenceBStep6, SUCCESS>, + Transition, StiStartSequenceBStep4, PREVIOUS>, + Transition, StiStartSequenceBStep6, NEXT>, + Transition, StiStartSequenceBStep8, SUCCESS> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_b/sti_start_sequence_b_step_6.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_b/sti_start_sequence_b_step_6.hpp new file mode 100644 index 000000000..eeec7eafa --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_b/sti_start_sequence_b_step_6.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace start_sequence_b +{ +// STATE DECLARATION +struct StiStartSequenceBStep6 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiStartSequenceBStep7, TIMEOUT>, + Transition, StiStartSequenceBStep5, PREVIOUS>, + Transition, StiStartSequenceBStep7, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_b/sti_start_sequence_b_step_7.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_b/sti_start_sequence_b_step_7.hpp new file mode 100644 index 000000000..c67c641f2 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_b/sti_start_sequence_b_step_7.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace start_sequence_b +{ +// STATE DECLARATION +struct StiStartSequenceBStep7 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiStartSequenceBStep8, TIMEOUT>, + Transition, StiStartSequenceBStep6, PREVIOUS>, + Transition, StiStartSequenceBStep8, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_b/sti_start_sequence_b_step_8.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_b/sti_start_sequence_b_step_8.hpp new file mode 100644 index 000000000..cfc4a685a --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_b/sti_start_sequence_b_step_8.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace start_sequence_b +{ +// STATE DECLARATION +struct StiStartSequenceBStep8 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiStartSequenceBStep9, TIMEOUT>, + Transition, StiStartSequenceBStep7, PREVIOUS>, + Transition, StiStartSequenceBStep9, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_b/sti_start_sequence_b_step_9.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_b/sti_start_sequence_b_step_9.hpp new file mode 100644 index 000000000..a596f385d --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_b/sti_start_sequence_b_step_9.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace start_sequence_b +{ +// STATE DECLARATION +struct StiStartSequenceBStep9 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiStartSequenceBLoop, TIMEOUT>, + Transition, StiStartSequenceBStep8, PREVIOUS>, + Transition, StiStartSequenceBLoop, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_b_loop.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_b_loop.hpp new file mode 100644 index 000000000..d658fad31 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_sequence_b_loop.hpp @@ -0,0 +1,52 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ + +// STATE DECLARATION +struct StartSequenceBLoop : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, SsStartSequenceB, CONTINUELOOP> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} + + bool loopWhileCondition() + { + auto & superstate = this->context(); + + RCLCPP_INFO( + getLogger(), "Loop start, current iterations: %d, total iterations: %d", + superstate.yiteration_count, superstate.ytotal_iterations()); + return superstate.yiteration_count++ < superstate.ytotal_iterations(); + } + + void onEntry() + { + RCLCPP_INFO(getLogger(), "LOOP START ON ENTRY"); + checkWhileLoopConditionAndThrowEvent(&StartSequenceBLoop::loopWhileCondition); + } +}; +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_st_observe.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_st_observe.hpp new file mode 100644 index 000000000..211fb08f6 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/start_st_observe.hpp @@ -0,0 +1,53 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +// STATE DECLARATION +struct StartStObserve : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct start_sequence_a : SUCCESS{}; + struct start_sequence_b : SUCCESS{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StartSequenceBLoop, SUCCESS>, + // Transition, SsExecuteSequenceA>, + // Keyboard events + // Transition, MsStarting, SUCCESS>, + Transition, StartSequenceALoop, SUCCESS>, + // Transition, SsExecuteSequenceA, execute_sequence_a>, + Transition, StartSequenceBLoop, SUCCESS> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(10); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_a/sti_suspending_sequence_a_loop.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_a/sti_suspending_sequence_a_loop.hpp new file mode 100644 index 000000000..9e742369b --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_a/sti_suspending_sequence_a_loop.hpp @@ -0,0 +1,54 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace suspending_sequence_a +{ +// STATE DECLARATION +struct StiSuspendingSequenceALoop : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSuspendingSequenceAStep1, CONTINUELOOP> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} + + bool loopWhileCondition() + { + auto & superstate = this->context(); + + RCLCPP_INFO( + getLogger(), "Loop start, current iterations: %d, total iterations: %d", + superstate.iteration_count, superstate.total_iterations()); + return superstate.iteration_count++ < superstate.total_iterations(); + } + + void onEntry() + { + RCLCPP_INFO(getLogger(), "LOOP START ON ENTRY"); + checkWhileLoopConditionAndThrowEvent(&StiSuspendingSequenceALoop::loopWhileCondition); + } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_a/sti_suspending_sequence_a_step_1.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_a/sti_suspending_sequence_a_step_1.hpp new file mode 100644 index 000000000..bb738b2b8 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_a/sti_suspending_sequence_a_step_1.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace suspending_sequence_a +{ +// STATE DECLARATION +struct StiSuspendingSequenceAStep1 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSuspendingSequenceAStep2, TIMEOUT>, + Transition, StiSuspendingSequenceAStep2, NEXT> + //Transition, StiStartSequenceALoop, PREVIOUS>, + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_a/sti_suspending_sequence_a_step_2.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_a/sti_suspending_sequence_a_step_2.hpp new file mode 100644 index 000000000..3b92d0169 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_a/sti_suspending_sequence_a_step_2.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace suspending_sequence_a +{ +// STATE DECLARATION +struct StiSuspendingSequenceAStep2 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSuspendingSequenceAStep3, TIMEOUT>, + Transition, StiSuspendingSequenceAStep1, PREVIOUS>, + Transition, StiSuspendingSequenceAStep3, NEXT> + + //Transition, ExecuteStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_a/sti_suspending_sequence_a_step_3.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_a/sti_suspending_sequence_a_step_3.hpp new file mode 100644 index 000000000..2835cf2d3 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_a/sti_suspending_sequence_a_step_3.hpp @@ -0,0 +1,56 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace suspending_sequence_a +{ +// STATE DECLARATION +struct StiSuspendingSequenceAStep3 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSuspendingSequenceAStep4, TIMEOUT>, + Transition, StiSuspendingSequenceAStep2, PREVIOUS>, + Transition, StiSuspendingSequenceAStep4, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_a/sti_suspending_sequence_a_step_4.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_a/sti_suspending_sequence_a_step_4.hpp new file mode 100644 index 000000000..8ce478ac2 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_a/sti_suspending_sequence_a_step_4.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace suspending_sequence_a +{ +// STATE DECLARATION +struct StiSuspendingSequenceAStep4 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSuspendingSequenceAStep5, TIMEOUT>, + Transition, StiSuspendingSequenceAStep3, PREVIOUS>, + Transition, StiSuspendingSequenceAStep5, NEXT> + + //Transition, StartStObserve, RETURN>, + // Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_a/sti_suspending_sequence_a_step_5.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_a/sti_suspending_sequence_a_step_5.hpp new file mode 100644 index 000000000..3bfd63a9c --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_a/sti_suspending_sequence_a_step_5.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace suspending_sequence_a +{ +// STATE DECLARATION +struct StiSuspendingSequenceAStep5 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSuspendingSequenceAStep6, TIMEOUT>, + Transition, StiSuspendingSequenceAStep4, PREVIOUS>, + Transition, StiSuspendingSequenceAStep6, NEXT> + + //Transition, ExecuteStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_a/sti_suspending_sequence_a_step_6.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_a/sti_suspending_sequence_a_step_6.hpp new file mode 100644 index 000000000..dadd93219 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_a/sti_suspending_sequence_a_step_6.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace suspending_sequence_a +{ +// STATE DECLARATION +struct StiSuspendingSequenceAStep6 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSuspendingSequenceAStep7, TIMEOUT>, + Transition, StiSuspendingSequenceAStep5, PREVIOUS>, + Transition, StiSuspendingSequenceAStep7, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_a/sti_suspending_sequence_a_step_7.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_a/sti_suspending_sequence_a_step_7.hpp new file mode 100644 index 000000000..ef06f7001 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_a/sti_suspending_sequence_a_step_7.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace suspending_sequence_a +{ +// STATE DECLARATION +struct StiSuspendingSequenceAStep7 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSuspendingSequenceAStep8, TIMEOUT>, + Transition, StiSuspendingSequenceAStep6, PREVIOUS>, + Transition, StiSuspendingSequenceAStep8, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_a/sti_suspending_sequence_a_step_8.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_a/sti_suspending_sequence_a_step_8.hpp new file mode 100644 index 000000000..109a2501a --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_a/sti_suspending_sequence_a_step_8.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace suspending_sequence_a +{ +// STATE DECLARATION +struct StiSuspendingSequenceAStep8 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSuspendingSequenceAStep9, TIMEOUT>, + Transition, StiSuspendingSequenceAStep7, PREVIOUS>, + Transition, StiSuspendingSequenceAStep9, NEXT> + + //Transition, ExecuteStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_a/sti_suspending_sequence_a_step_9.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_a/sti_suspending_sequence_a_step_9.hpp new file mode 100644 index 000000000..d2a407955 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_a/sti_suspending_sequence_a_step_9.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace suspending_sequence_a +{ +// STATE DECLARATION +struct StiSuspendingSequenceAStep9 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSuspendingSequenceALoop, TIMEOUT>, + Transition, StiSuspendingSequenceAStep8, PREVIOUS>, + Transition, StiSuspendingSequenceALoop, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_a_loop.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_a_loop.hpp new file mode 100644 index 000000000..b87620685 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_a_loop.hpp @@ -0,0 +1,52 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ + +// STATE DECLARATION +struct SuspendingSequenceALoop : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, SsSuspendingSequenceA, CONTINUELOOP> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} + + bool loopWhileCondition() + { + auto & superstate = this->context(); + + RCLCPP_INFO( + getLogger(), "Loop start, current iterations: %d, total iterations: %d", + superstate.ziteration_count, superstate.ztotal_iterations()); + return superstate.ziteration_count++ < superstate.ztotal_iterations(); + } + + void onEntry() + { + RCLCPP_INFO(getLogger(), "LOOP START ON ENTRY"); + checkWhileLoopConditionAndThrowEvent(&SuspendingSequenceALoop::loopWhileCondition); + } +}; +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_b/sti_suspending_sequence_b_loop.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_b/sti_suspending_sequence_b_loop.hpp new file mode 100644 index 000000000..34e5fbe2a --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_b/sti_suspending_sequence_b_loop.hpp @@ -0,0 +1,54 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace suspending_sequence_b +{ +// STATE DECLARATION +struct StiSuspendingSequenceB : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSuspendingSequenceBStep1, CONTINUELOOP> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} + + bool loopWhileCondition() + { + auto & superstate = this->context(); + + RCLCPP_INFO( + getLogger(), "Loop start, current iterations: %d, total iterations: %d", + superstate.iteration_count, superstate.total_iterations()); + return superstate.iteration_count++ < superstate.total_iterations(); + } + + void onEntry() + { + RCLCPP_INFO(getLogger(), "LOOP START ON ENTRY"); + checkWhileLoopConditionAndThrowEvent(&StiSuspendingSequenceB::loopWhileCondition); + } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_b/sti_suspending_sequence_b_step_1.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_b/sti_suspending_sequence_b_step_1.hpp new file mode 100644 index 000000000..3613e9072 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_b/sti_suspending_sequence_b_step_1.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace suspending_sequence_b +{ +// STATE DECLARATION +struct StiSuspendingSequenceBStep1 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSuspendingSequenceBStep2, TIMEOUT>, + Transition, StiSuspendingSequenceBStep2, NEXT>, + Transition, StiSuspendingSequenceB, PREVIOUS> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_b/sti_suspending_sequence_b_step_2.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_b/sti_suspending_sequence_b_step_2.hpp new file mode 100644 index 000000000..827bf318c --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_b/sti_suspending_sequence_b_step_2.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace suspending_sequence_b +{ +// STATE DECLARATION +struct StiSuspendingSequenceBStep2 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSuspendingSequenceBStep3, TIMEOUT>, + Transition, StiSuspendingSequenceBStep1, PREVIOUS>, + Transition, StiSuspendingSequenceBStep3, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_b/sti_suspending_sequence_b_step_3.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_b/sti_suspending_sequence_b_step_3.hpp new file mode 100644 index 000000000..d714a121e --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_b/sti_suspending_sequence_b_step_3.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace suspending_sequence_b +{ +// STATE DECLARATION +struct StiSuspendingSequenceBStep3 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSuspendingSequenceBStep4, TIMEOUT>, + Transition, StiSuspendingSequenceBStep2, PREVIOUS>, + Transition, StiSuspendingSequenceBStep4, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_b/sti_suspending_sequence_b_step_4.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_b/sti_suspending_sequence_b_step_4.hpp new file mode 100644 index 000000000..e54db8286 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_b/sti_suspending_sequence_b_step_4.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace suspending_sequence_b +{ +// STATE DECLARATION +struct StiSuspendingSequenceBStep4 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSuspendingSequenceBStep5, TIMEOUT>, + Transition, StiSuspendingSequenceBStep3, PREVIOUS>, + Transition, StiSuspendingSequenceBStep5, NEXT> + + //ransition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_b/sti_suspending_sequence_b_step_5.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_b/sti_suspending_sequence_b_step_5.hpp new file mode 100644 index 000000000..73b1b634a --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_b/sti_suspending_sequence_b_step_5.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace suspending_sequence_b +{ +// STATE DECLARATION +struct StiSuspendingSequenceBStep5 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSuspendingSequenceBStep6, TIMEOUT>, + Transition, StiSuspendingSequenceBStep4, PREVIOUS>, + Transition, StiSuspendingSequenceBStep6, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_b/sti_suspending_sequence_b_step_6.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_b/sti_suspending_sequence_b_step_6.hpp new file mode 100644 index 000000000..7ee80ad1a --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_b/sti_suspending_sequence_b_step_6.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace suspending_sequence_b +{ +// STATE DECLARATION +struct StiSuspendingSequenceBStep6 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSuspendingSequenceBStep7, TIMEOUT>, + Transition, StiSuspendingSequenceBStep5, PREVIOUS>, + Transition, StiSuspendingSequenceBStep7, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_b/sti_suspending_sequence_b_step_7.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_b/sti_suspending_sequence_b_step_7.hpp new file mode 100644 index 000000000..a5460211b --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_b/sti_suspending_sequence_b_step_7.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace suspending_sequence_b +{ +// STATE DECLARATION +struct StiSuspendingSequenceBStep7 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSuspendingSequenceBStep8, TIMEOUT>, + Transition, StiSuspendingSequenceBStep6, PREVIOUS>, + Transition, StiSuspendingSequenceBStep8, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_b/sti_suspending_sequence_b_step_8.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_b/sti_suspending_sequence_b_step_8.hpp new file mode 100644 index 000000000..2a1a33943 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_b/sti_suspending_sequence_b_step_8.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace suspending_sequence_b +{ +// STATE DECLARATION +struct StiSuspendingSequenceBStep8 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSuspendingSequenceBStep9, TIMEOUT>, + Transition, StiSuspendingSequenceBStep7, PREVIOUS>, + Transition, StiSuspendingSequenceBStep9, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_b/sti_suspending_sequence_b_step_9.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_b/sti_suspending_sequence_b_step_9.hpp new file mode 100644 index 000000000..92ee96231 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_b/sti_suspending_sequence_b_step_9.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace suspending_sequence_b +{ +// STATE DECLARATION +struct StiSuspendingSequenceBStep9 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSuspendingSequenceB, TIMEOUT>, + Transition, StiSuspendingSequenceBStep8, PREVIOUS>, + Transition, StiSuspendingSequenceB, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace execute_sequence_b +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_b_loop.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_b_loop.hpp new file mode 100644 index 000000000..d2bbb57ea --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_b_loop.hpp @@ -0,0 +1,52 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ + +// STATE DECLARATION +struct SuspendingSequenceB : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, SsSuspendingSequenceB, CONTINUELOOP> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} + + bool loopWhileCondition() + { + auto & superstate = this->context(); + + RCLCPP_INFO( + getLogger(), "Loop start, current iterations: %d, total iterations: %d", + superstate.yiteration_count, superstate.ytotal_iterations()); + return superstate.yiteration_count++ < superstate.ytotal_iterations(); + } + + void onEntry() + { + RCLCPP_INFO(getLogger(), "LOOP START ON ENTRY"); + checkWhileLoopConditionAndThrowEvent(&SuspendingSequenceB::loopWhileCondition); + } +}; +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_c/sti_suspending_sequence_c_loop.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_c/sti_suspending_sequence_c_loop.hpp new file mode 100644 index 000000000..d75c8962f --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_c/sti_suspending_sequence_c_loop.hpp @@ -0,0 +1,54 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace suspending_sequence_c +{ +// STATE DECLARATION +struct StiSuspendingSequenceCLoop : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSuspendingSequenceCStep1, CONTINUELOOP> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} + + bool loopWhileCondition() + { + auto & superstate = this->context(); + + RCLCPP_INFO( + getLogger(), "Loop start, current iterations: %d, total iterations: %d", + superstate.diteration_count, superstate.dtotal_iterations()); + return superstate.diteration_count++ < superstate.dtotal_iterations(); + } + + void onEntry() + { + RCLCPP_INFO(getLogger(), "LOOP START ON ENTRY"); + checkWhileLoopConditionAndThrowEvent(&StiSuspendingSequenceCLoop::loopWhileCondition); + } +}; +} // namespace suspending_sequence_c_1 +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_c/sti_suspending_sequence_c_step_1.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_c/sti_suspending_sequence_c_step_1.hpp new file mode 100644 index 000000000..4c6aa23e6 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_c/sti_suspending_sequence_c_step_1.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace suspending_sequence_c +{ +// STATE DECLARATION +struct StiSuspendingSequenceCStep1 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSuspendingSequenceCStep2, TIMEOUT>, + Transition, StiSuspendingSequenceCStep2, NEXT> + //Transition, StiSuspendingSequenceCLoop2, PREVIOUS>, + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace suspending_sequence_c_1 +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_c/sti_suspending_sequence_c_step_2.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_c/sti_suspending_sequence_c_step_2.hpp new file mode 100644 index 000000000..880b6ce1c --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_c/sti_suspending_sequence_c_step_2.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace suspending_sequence_c +{ +// STATE DECLARATION +struct StiSuspendingSequenceCStep2 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSuspendingSequenceCStep3, TIMEOUT>, + Transition, StiSuspendingSequenceCStep1, PREVIOUS>, + Transition, StiSuspendingSequenceCStep3, NEXT> + + //Transition, ExecuteStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace suspending_sequence_c_1 +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_c/sti_suspending_sequence_c_step_3.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_c/sti_suspending_sequence_c_step_3.hpp new file mode 100644 index 000000000..b707d6353 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_c/sti_suspending_sequence_c_step_3.hpp @@ -0,0 +1,56 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace suspending_sequence_c +{ +// STATE DECLARATION +struct StiSuspendingSequenceCStep3 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSuspendingSequenceCStep4, TIMEOUT>, + Transition, StiSuspendingSequenceCStep2, PREVIOUS>, + Transition, StiSuspendingSequenceCStep4, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace suspending_sequence_c_1 +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_c/sti_suspending_sequence_c_step_4.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_c/sti_suspending_sequence_c_step_4.hpp new file mode 100644 index 000000000..e60e2948a --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_c/sti_suspending_sequence_c_step_4.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace suspending_sequence_c +{ +// STATE DECLARATION +struct StiSuspendingSequenceCStep4 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSuspendingSequenceCStep5, TIMEOUT>, + Transition, StiSuspendingSequenceCStep3, PREVIOUS>, + Transition, StiSuspendingSequenceCStep5, NEXT> + + //Transition, StartStObserve, RETURN>, + // Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace suspending_sequence_c_1 +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_c/sti_suspending_sequence_c_step_5.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_c/sti_suspending_sequence_c_step_5.hpp new file mode 100644 index 000000000..f6631522d --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_c/sti_suspending_sequence_c_step_5.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace suspending_sequence_c +{ +// STATE DECLARATION +struct StiSuspendingSequenceCStep5 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSuspendingSequenceCStep6, TIMEOUT>, + Transition, StiSuspendingSequenceCStep4, PREVIOUS>, + Transition, StiSuspendingSequenceCStep6, NEXT> + + //Transition, ExecuteStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace suspending_sequence_c_1 +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_c/sti_suspending_sequence_c_step_6.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_c/sti_suspending_sequence_c_step_6.hpp new file mode 100644 index 000000000..6be05d7f1 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_c/sti_suspending_sequence_c_step_6.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace suspending_sequence_c +{ +// STATE DECLARATION +struct StiSuspendingSequenceCStep6 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSuspendingSequenceCStep7, TIMEOUT>, + Transition, StiSuspendingSequenceCStep5, PREVIOUS>, + Transition, StiSuspendingSequenceCStep7, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace suspending_sequence_c_1 +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_c/sti_suspending_sequence_c_step_7.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_c/sti_suspending_sequence_c_step_7.hpp new file mode 100644 index 000000000..49b15f8e7 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_c/sti_suspending_sequence_c_step_7.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace suspending_sequence_c +{ +// STATE DECLARATION +struct StiSuspendingSequenceCStep7 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSuspendingSequenceCStep9, TIMEOUT>, + Transition, StiSuspendingSequenceCStep6, PREVIOUS>, + Transition, StiSuspendingSequenceCStep9, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace suspending_sequence_c_1 +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_c/sti_suspending_sequence_c_step_8.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_c/sti_suspending_sequence_c_step_8.hpp new file mode 100644 index 000000000..1ed14d028 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_c/sti_suspending_sequence_c_step_8.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace suspending_sequence_c +{ +// STATE DECLARATION +struct StiSuspendingSequenceCStep8 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSuspendingSequenceCStep9, TIMEOUT>, + Transition, StiSuspendingSequenceCStep7, PREVIOUS>, + Transition, StiSuspendingSequenceCStep9, NEXT> + + //Transition, ExecuteStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace suspending_sequence_c_1 +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_c/sti_suspending_sequence_c_step_9.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_c/sti_suspending_sequence_c_step_9.hpp new file mode 100644 index 000000000..d21d3df6c --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_c/sti_suspending_sequence_c_step_9.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace suspending_sequence_c +{ +// STATE DECLARATION +struct StiSuspendingSequenceCStep9 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSuspendingSequenceCLoop, TIMEOUT>, + Transition, StiSuspendingSequenceCStep7, PREVIOUS>, + Transition, StiSuspendingSequenceCLoop, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace suspending_sequence_c_1 +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_c_loop.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_c_loop.hpp new file mode 100644 index 000000000..78e5b4dd8 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_c_loop.hpp @@ -0,0 +1,52 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ + +// STATE DECLARATION +struct SuspendingSequenceCLoop : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, SsSuspendingSequenceC, CONTINUELOOP> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} + + bool loopWhileCondition() + { + auto & superstate = this->context(); + + RCLCPP_INFO( + getLogger(), "Loop start, current iterations: %d, total iterations: %d", + superstate.diteration_count, superstate.dtotal_iterations()); + return superstate.diteration_count++ < superstate.dtotal_iterations(); + } + + void onEntry() + { + RCLCPP_INFO(getLogger(), "LOOP START ON ENTRY"); + checkWhileLoopConditionAndThrowEvent(&SuspendingSequenceCLoop::loopWhileCondition); + } +}; +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_d/sti_suspending_sequence_d_loop.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_d/sti_suspending_sequence_d_loop.hpp new file mode 100644 index 000000000..3dadb44b7 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_d/sti_suspending_sequence_d_loop.hpp @@ -0,0 +1,54 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace suspending_sequence_d +{ +// STATE DECLARATION +struct StiSuspendingSequenceDLoop : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSuspendingSequenceDStep1, CONTINUELOOP> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} + + bool loopWhileCondition() + { + auto & superstate = this->context(); + + RCLCPP_INFO( + getLogger(), "Loop start, current iterations: %d, total iterations: %d", + superstate.giteration_count, superstate.gtotal_iterations()); + return superstate.giteration_count++ < superstate.gtotal_iterations(); + } + + void onEntry() + { + RCLCPP_INFO(getLogger(), "LOOP START ON ENTRY"); + checkWhileLoopConditionAndThrowEvent(&StiSuspendingSequenceDLoop::loopWhileCondition); + } +}; +} // namespace execute_sequence_a +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_d/sti_suspending_sequence_d_step_1.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_d/sti_suspending_sequence_d_step_1.hpp new file mode 100644 index 000000000..da8a3cd0d --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_d/sti_suspending_sequence_d_step_1.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace suspending_sequence_d +{ +// STATE DECLARATION +struct StiSuspendingSequenceDStep1 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSuspendingSequenceDStep2, TIMEOUT>, + Transition, StiSuspendingSequenceDStep2, NEXT> + //Transition, StiStartSequenceALoop, PREVIOUS>, + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace suspending_sequence_d +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_d/sti_suspending_sequence_d_step_2.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_d/sti_suspending_sequence_d_step_2.hpp new file mode 100644 index 000000000..8a5c0af06 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_d/sti_suspending_sequence_d_step_2.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace suspending_sequence_d +{ +// STATE DECLARATION +struct StiSuspendingSequenceDStep2 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSuspendingSequenceDStep3, TIMEOUT>, + Transition, StiSuspendingSequenceDStep1, PREVIOUS>, + Transition, StiSuspendingSequenceDStep3, NEXT> + + //Transition, ExecuteStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace suspending_sequence_d +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_d/sti_suspending_sequence_d_step_3.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_d/sti_suspending_sequence_d_step_3.hpp new file mode 100644 index 000000000..f69399224 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_d/sti_suspending_sequence_d_step_3.hpp @@ -0,0 +1,56 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace suspending_sequence_d +{ +// STATE DECLARATION +struct StiSuspendingSequenceDStep3 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSuspendingSequenceDStep4, TIMEOUT>, + Transition, StiSuspendingSequenceDStep2, PREVIOUS>, + Transition, StiSuspendingSequenceDStep4, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace suspending_sequence_d +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_d/sti_suspending_sequence_d_step_4.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_d/sti_suspending_sequence_d_step_4.hpp new file mode 100644 index 000000000..5baae9ac0 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_d/sti_suspending_sequence_d_step_4.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace suspending_sequence_d +{ +// STATE DECLARATION +struct StiSuspendingSequenceDStep4 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSuspendingSequenceDStep5, TIMEOUT>, + Transition, StiSuspendingSequenceDStep3, PREVIOUS>, + Transition, StiSuspendingSequenceDStep5, NEXT> + + //Transition, StartStObserve, RETURN>, + // Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace suspending_sequence_d +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_d/sti_suspending_sequence_d_step_5.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_d/sti_suspending_sequence_d_step_5.hpp new file mode 100644 index 000000000..a915615c7 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_d/sti_suspending_sequence_d_step_5.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace suspending_sequence_d +{ +// STATE DECLARATION +struct StiSuspendingSequenceDStep5 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSuspendingSequenceDStep6, TIMEOUT>, + Transition, StiSuspendingSequenceDStep4, PREVIOUS>, + Transition, StiSuspendingSequenceDStep6, NEXT> + + //Transition, ExecuteStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace suspending_sequence_d +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_d/sti_suspending_sequence_d_step_6.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_d/sti_suspending_sequence_d_step_6.hpp new file mode 100644 index 000000000..93392a484 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_d/sti_suspending_sequence_d_step_6.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace suspending_sequence_d +{ +// STATE DECLARATION +struct StiSuspendingSequenceDStep6 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSuspendingSequenceDStep7, TIMEOUT>, + Transition, StiSuspendingSequenceDStep5, PREVIOUS>, + Transition, StiSuspendingSequenceDStep7, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace suspending_sequence_d +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_d/sti_suspending_sequence_d_step_7.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_d/sti_suspending_sequence_d_step_7.hpp new file mode 100644 index 000000000..85c0b96da --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_d/sti_suspending_sequence_d_step_7.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace suspending_sequence_d +{ +// STATE DECLARATION +struct StiSuspendingSequenceDStep7 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSuspendingSequenceDStep8, TIMEOUT>, + Transition, StiSuspendingSequenceDStep6, PREVIOUS>, + Transition, StiSuspendingSequenceDStep8, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace suspending_sequence_d +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_d/sti_suspending_sequence_d_step_8.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_d/sti_suspending_sequence_d_step_8.hpp new file mode 100644 index 000000000..1f0ec1575 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_d/sti_suspending_sequence_d_step_8.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace suspending_sequence_d +{ +// STATE DECLARATION +struct StiSuspendingSequenceDStep8 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSuspendingSequenceDStep9, TIMEOUT>, + Transition, StiSuspendingSequenceDStep7, PREVIOUS>, + Transition, StiSuspendingSequenceDStep9, NEXT> + + //Transition, ExecuteStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace suspending_sequence_d +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_d/sti_suspending_sequence_d_step_9.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_d/sti_suspending_sequence_d_step_9.hpp new file mode 100644 index 000000000..4c36f066a --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_d/sti_suspending_sequence_d_step_9.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +namespace suspending_sequence_d +{ +// STATE DECLARATION +struct StiSuspendingSequenceDStep9 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct TIMEOUT : ABORT{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + struct RETURN : CANCEL{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSuspendingSequenceDLoop, TIMEOUT>, + Transition, StiSuspendingSequenceDStep8, PREVIOUS>, + Transition, StiSuspendingSequenceDLoop, NEXT> + + //Transition, StartStObserve, RETURN>, + //Transition, MsRecovery2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(20); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace suspending_sequence_d +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_d_loop.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_d_loop.hpp new file mode 100644 index 000000000..7ce7dc896 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_sequence_d_loop.hpp @@ -0,0 +1,52 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ + +// STATE DECLARATION +struct SuspendingSequenceDLoop : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, SsSuspendingSequenceD, CONTINUELOOP> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} + + bool loopWhileCondition() + { + auto & superstate = this->context(); + + RCLCPP_INFO( + getLogger(), "Loop start, current iterations: %d, total iterations: %d", + superstate.giteration_count, superstate.gtotal_iterations()); + return superstate.giteration_count++ < superstate.gtotal_iterations(); + } + + void onEntry() + { + RCLCPP_INFO(getLogger(), "LOOP START ON ENTRY"); + checkWhileLoopConditionAndThrowEvent(&SuspendingSequenceDLoop::loopWhileCondition); + } +}; +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_st_observe.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_st_observe.hpp new file mode 100644 index 000000000..8589b9aa9 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/states/suspending_st_observe.hpp @@ -0,0 +1,53 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +// STATE DECLARATION +struct SuspendingStObserve : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct suspending_sequence_a : SUCCESS{}; + struct suspending_sequence_c : SUCCESS{}; + struct suspending_sequence_d : SUCCESS{}; + struct suspending_sequence_b : SUCCESS{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, SuspendingSequenceALoop, SUCCESS>, + Transition, SuspendingSequenceB, SUCCESS>, + Transition, SuspendingSequenceCLoop, SUCCESS>, + Transition, SuspendingSequenceDLoop, SUCCESS>, + Transition, SuspendingSequenceDLoop, SUCCESS> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(10); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/superstates/ss_completing_sequence_a.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/superstates/ss_completing_sequence_a.hpp new file mode 100644 index 000000000..cde5aa9f5 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/superstates/ss_completing_sequence_a.hpp @@ -0,0 +1,42 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +using namespace sm_multi_panda_sim::completing_sequence_a; + +// STATE DECLARATION +struct SsCompletingSequenceA : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, CompletingSequenceALoop> + + >reactions; + + // STATE VARIABLES + static constexpr int total_iterations() { return 1; } + int iteration_count = 0; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} +}; // namespace SS1 + +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/superstates/ss_completing_sequence_b.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/superstates/ss_completing_sequence_b.hpp new file mode 100644 index 000000000..ec4ff36b2 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/superstates/ss_completing_sequence_b.hpp @@ -0,0 +1,42 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +using namespace sm_multi_panda_sim::completing_sequence_b; + +// STATE DECLARATION +struct SsCompletingSequenceB : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, CompletingStObserve> + + >reactions; + + // STATE VARIABLES + static constexpr int total_iterations() { return 1; } + int iteration_count = 0; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} +}; // namespace SS4 + +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/superstates/ss_execute_sequence_a.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/superstates/ss_execute_sequence_a.hpp new file mode 100644 index 000000000..5452b2d5b --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/superstates/ss_execute_sequence_a.hpp @@ -0,0 +1,42 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +using namespace sm_multi_panda_sim::execute_sequence_a; + +// STATE DECLARATION +struct SsExecuteSequenceA : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, ExecuteSequenceALoop> + + >reactions; + + // STATE VARIABLES + static constexpr int total_iterations() { return 1; } + int iteration_count = 0; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} +}; // namespace SS1 + +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/superstates/ss_execute_sequence_b.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/superstates/ss_execute_sequence_b.hpp new file mode 100644 index 000000000..6fee18551 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/superstates/ss_execute_sequence_b.hpp @@ -0,0 +1,42 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +using namespace sm_multi_panda_sim::execute_sequence_b; + +// STATE DECLARATION +struct SsExecuteSequenceB : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, ExecuteStObserve> + + >reactions; + + // STATE VARIABLES + static constexpr int total_iterations() { return 1; } + int iteration_count = 0; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} +}; // namespace SS4 + +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/superstates/ss_holding_sequence_a.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/superstates/ss_holding_sequence_a.hpp new file mode 100644 index 000000000..833db18cb --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/superstates/ss_holding_sequence_a.hpp @@ -0,0 +1,42 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +using namespace sm_multi_panda_sim::holding_sequence_a; + +// STATE DECLARATION +struct SsHoldingSequenceA : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, HoldingSequenceALoop> + + >reactions; + + // STATE VARIABLES + static constexpr int total_iterations() { return 1; } + int iteration_count = 0; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} +}; // namespace SS1 + +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/superstates/ss_holding_sequence_b.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/superstates/ss_holding_sequence_b.hpp new file mode 100644 index 000000000..623f23759 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/superstates/ss_holding_sequence_b.hpp @@ -0,0 +1,42 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +using namespace sm_multi_panda_sim::holding_sequence_b; + +// STATE DECLARATION +struct SsHoldingSequenceB : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, HoldingStObserve> + + >reactions; + + // STATE VARIABLES + static constexpr int total_iterations() { return 1; } + int iteration_count = 0; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} +}; // namespace SS4 + +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/superstates/ss_start_sequence_a.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/superstates/ss_start_sequence_a.hpp new file mode 100644 index 000000000..f35061093 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/superstates/ss_start_sequence_a.hpp @@ -0,0 +1,42 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +using namespace sm_multi_panda_sim::start_sequence_a; + +// STATE DECLARATION +struct SsStartSequenceA : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StartSequenceALoop> + + >reactions; + + // STATE VARIABLES + static constexpr int total_iterations() { return 1; } + int iteration_count = 0; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} +}; // namespace SS1 + +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/superstates/ss_start_sequence_b.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/superstates/ss_start_sequence_b.hpp new file mode 100644 index 000000000..0219fa7d8 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/superstates/ss_start_sequence_b.hpp @@ -0,0 +1,42 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +using namespace sm_multi_panda_sim::start_sequence_b; + +// STATE DECLARATION +struct SsStartSequenceB : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StartStObserve> + + >reactions; + + // STATE VARIABLES + static constexpr int total_iterations() { return 1; } + int iteration_count = 0; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} +}; // namespace SS4 + +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/superstates/ss_suspending_sequence_a.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/superstates/ss_suspending_sequence_a.hpp new file mode 100644 index 000000000..4ca164fe0 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/superstates/ss_suspending_sequence_a.hpp @@ -0,0 +1,42 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +using namespace sm_multi_panda_sim::suspending_sequence_a; + +// STATE DECLARATION +struct SsSuspendingSequenceA : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, SuspendingSequenceALoop> + + >reactions; + + // STATE VARIABLES + static constexpr int total_iterations() { return 1; } + int iteration_count = 0; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} +}; // namespace SS1 + +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/superstates/ss_suspending_sequence_b.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/superstates/ss_suspending_sequence_b.hpp new file mode 100644 index 000000000..99aae8a20 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/superstates/ss_suspending_sequence_b.hpp @@ -0,0 +1,42 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +using namespace sm_multi_panda_sim::suspending_sequence_b; + +// STATE DECLARATION +struct SsSuspendingSequenceB : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, SuspendingStObserve> + + >reactions; + + // STATE VARIABLES + static constexpr int total_iterations() { return 1; } + int iteration_count = 0; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} +}; // namespace SS4 + +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/superstates/ss_suspending_sequence_c.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/superstates/ss_suspending_sequence_c.hpp new file mode 100644 index 000000000..e7c62e67a --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/superstates/ss_suspending_sequence_c.hpp @@ -0,0 +1,42 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +using namespace sm_multi_panda_sim::suspending_sequence_c; + +// STATE DECLARATION +struct SsSuspendingSequenceC : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, SuspendingSequenceCLoop> + + >reactions; + + // STATE VARIABLES + static constexpr int dtotal_iterations() { return 1; } + int diteration_count = 0; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} +}; // namespace SS1 + +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/superstates/ss_suspending_sequence_d.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/superstates/ss_suspending_sequence_d.hpp new file mode 100644 index 000000000..6c16888b5 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/include/sm_multi_panda_sim/superstates/ss_suspending_sequence_d.hpp @@ -0,0 +1,42 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +namespace sm_multi_panda_sim +{ +using namespace sm_multi_panda_sim::suspending_sequence_d; + +// STATE DECLARATION +struct SsSuspendingSequenceD : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, SuspendingSequenceDLoop> + + >reactions; + + // STATE VARIABLES + static constexpr int gtotal_iterations() { return 1; } + int giteration_count = 0; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} +}; // namespace SS1 + +} // namespace sm_multi_panda_sim diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/launch/moveit.rviz b/smacc2_sm_reference_library/sm_multi_panda_sim/launch/moveit.rviz new file mode 100644 index 000000000..aab03891f --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/launch/moveit.rviz @@ -0,0 +1,631 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 327 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 100 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /rviz_visual_tools + Value: true + - Class: moveit_rviz_plugin/Trajectory + Color Enabled: false + Enabled: true + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + left_panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + left_panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + right_panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Loop Animation: false + Name: Trajectory + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Robot Description: robot_description + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: /display_planned_path + Use Sim Time: false + Value: true + - Class: moveit_rviz_plugin/PlanningScene + Enabled: true + Move Group Namespace: "" + Name: PlanningScene + Planning Scene Topic: /monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + - Acceleration_Scaling_Factor: 0.1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: false + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Loop Animation: false + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 3x + Trail Step Size: 1 + Trajectory Topic: /display_planned_path + Use Sim Time: false + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0.20000000298023224 + Joint Violation Color: 255; 0; 255 + Planning Group: left_panda_arm + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: /monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 0.1 + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + left_panda_hand: + Value: true + left_panda_leftfinger: + Value: true + left_panda_link0: + Value: true + left_panda_link1: + Value: true + left_panda_link2: + Value: true + left_panda_link3: + Value: true + left_panda_link4: + Value: true + left_panda_link5: + Value: true + left_panda_link6: + Value: true + left_panda_link7: + Value: true + left_panda_link8: + Value: true + left_panda_rightfinger: + Value: true + right_panda_hand: + Value: true + right_panda_leftfinger: + Value: true + right_panda_link0: + Value: true + right_panda_link1: + Value: true + right_panda_link2: + Value: true + right_panda_link3: + Value: true + right_panda_link4: + Value: true + right_panda_link5: + Value: true + right_panda_link6: + Value: true + right_panda_link7: + Value: true + right_panda_link8: + Value: true + right_panda_rightfinger: + Value: true + world: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + world: + left_panda_link0: + left_panda_link1: + left_panda_link2: + left_panda_link3: + left_panda_link4: + left_panda_link5: + left_panda_link6: + left_panda_link7: + left_panda_link8: + left_panda_hand: + left_panda_leftfinger: + {} + left_panda_rightfinger: + {} + right_panda_link0: + right_panda_link1: + right_panda_link2: + right_panda_link3: + right_panda_link4: + right_panda_link5: + right_panda_link6: + right_panda_link7: + right_panda_link8: + right_panda_hand: + right_panda_leftfinger: + {} + right_panda_rightfinger: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + left_panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + left_panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + right_panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 3.534968852996826 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.21754494309425354 + Y: 0.01349867694079876 + Z: 0.40449175238609314 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.09039856493473053 + Target Frame: + Value: Orbit (rviz) + Yaw: 5.268585205078125 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1016 + Hide Left Dock: false + Hide Right Dock: true + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 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 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Trajectory - Trajectory Slider: + collapsed: false + Views: + collapsed: true + Width: 1848 + X: 72 + Y: 27 diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/launch/sm_multi_panda_sim.launch.py b/smacc2_sm_reference_library/sm_multi_panda_sim/launch/sm_multi_panda_sim.launch.py new file mode 100644 index 000000000..8bad48099 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/launch/sm_multi_panda_sim.launch.py @@ -0,0 +1,138 @@ +# Copyright (c) 2018 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +from launch import LaunchDescription + +# from launch.actions import DeclareLaunchArgument +# from launch.substitutions import LaunchConfiguration +# from launch.conditions import IfCondition, UnlessCondition +from launch_ros.actions import Node +from launch.actions import ExecuteProcess +from ament_index_python.packages import get_package_share_directory +from moveit_configs_utils import MoveItConfigsBuilder + + +def generate_launch_description(): + + moveit_config = ( + MoveItConfigsBuilder("dual_panda", package_name="sm_multi_panda_sim") + .robot_description(file_path="config/panda.urdf.xacro") + .robot_description_semantic(file_path="config/panda.srdf") + .trajectory_execution(file_path="config/moveit_controllers.yaml") + .planning_pipelines(pipelines=["ompl"]) + .moveit_cpp(file_path="config/moveit_cpp.yaml") + .to_moveit_configs() + ) + + xtermprefix = ( + "xterm -xrm 'XTerm*scrollBar: true' -xrm 'xterm*rightScrollBar: true' " + "-hold -geometry 1000x600 -sl 10000 -e" + ) + + # Start the actual move_group node/action server + move_group_node = Node( + package="moveit_ros_move_group", + executable="move_group", + output="screen", + prefix=xtermprefix, + parameters=[moveit_config.to_dict()], + ) + + # RViz + rviz_config = os.path.join( + get_package_share_directory("sm_multi_panda_sim"), + "launch/moveit.rviz", + ) + + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config], + parameters=[ + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.planning_pipelines, + moveit_config.robot_description_kinematics, + ], + ) + + sm_multi_panda_sim = Node( + package="sm_multi_panda_sim", + executable="sm_multi_panda_sim_node", + prefix=xtermprefix, + parameters=[moveit_config.to_dict()], + ) + + # MoveItCpp demo executable + # moveit_cpp_node = Node( + # name="moveit_cpp_tutorial", + # package="sm_multi_panda_sim", + # executable="moveit_cpp_tutorial_dual", + # output="screen", + # prefix = xtermprefix, + # parameters=[moveit_config.to_dict()], + # ) + + # Publish TF + robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="both", + parameters=[moveit_config.robot_description], + ) + + # ros2_control using FakeSystem as hardware + ros2_controllers_path = os.path.join( + get_package_share_directory("sm_multi_panda_sim"), + "config/", + "ros2_controllers.yaml", + ) + ros2_control_node = Node( + package="controller_manager", + executable="ros2_control_node", + prefix=xtermprefix, + parameters=[moveit_config.robot_description, ros2_controllers_path], + output="both", + ) + + # Load controllers + load_controllers = [] + for controller in [ + "joint_state_broadcaster", + "left_arm_controller", + "right_arm_controller", + ]: + load_controllers += [ + ExecuteProcess( + cmd=[f"ros2 run controller_manager spawner {controller}"], + shell=True, + output="screen", + ) + ] + + return LaunchDescription( + [ + rviz_node, + robot_state_publisher, + move_group_node, + ros2_control_node, + # moveit_cpp_node + sm_multi_panda_sim, + ] + + load_controllers + ) diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/package.xml b/smacc2_sm_reference_library/sm_multi_panda_sim/package.xml new file mode 100644 index 000000000..ece0c0805 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/package.xml @@ -0,0 +1,33 @@ + + + + sm_multi_panda_sim + 0.0.0 + The sm_multi_panda_sim package + SMACC2 is cool + Apache-2.0 + MyLicense + + ament_cmake + + libboost-thread-dev + + ros_timer_client + smacc2 + move_group_interface_client + controller_manager + warehouse_ros_mongo + moveit_configs_utils + moveit_resources_panda_description + + sr_all_events_go + ros_publisher_client + keyboard_client + xterm + + + + + ament_cmake + + diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/rviz/.empty b/smacc2_sm_reference_library/sm_multi_panda_sim/rviz/.empty new file mode 100644 index 000000000..e69de29bb diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/src/sm_multi_panda_sim/sm_multi_panda_sim_node.cpp b/smacc2_sm_reference_library/sm_multi_panda_sim/src/sm_multi_panda_sim/sm_multi_panda_sim_node.cpp new file mode 100644 index 000000000..3d5a7f879 --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/src/sm_multi_panda_sim/sm_multi_panda_sim_node.cpp @@ -0,0 +1,27 @@ +// Copyright 2021 MyName/MyCompany Inc. +// Copyright 2021 RobosoftAI Inc. (template) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// +// Author: Denis Štogl (template) +// + +#include + +//-------------------------------------------------------------------- +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + smacc2::run(); +}