From 53bd58907a68e43a0347685f81214e68f076573c Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Fri, 9 Jun 2023 23:40:02 +0000 Subject: [PATCH 01/70] fix navigation humble --- .../client_bases/smacc_action_client_base.hpp | 4 +- .../smacc2/impl/smacc_state_machine_impl.hpp | 7 +++ smacc2/include/smacc2/smacc_state_machine.hpp | 2 + .../cp_waypoints_navigator.hpp | 8 +-- .../client_behaviors/cb_absolute_rotate.cpp | 2 +- .../cb_nav2z_client_behavior_base.cpp | 2 + .../client_behaviors/cb_navigate_backward.cpp | 2 +- .../client_behaviors/cb_navigate_forward.cpp | 6 ++- .../cb_navigate_global_position.cpp | 2 +- .../client_behaviors/cb_rotate.cpp | 2 +- .../cb_undo_path_backwards.cpp | 8 ++- .../cp_waypoints_navigator.cpp | 54 +++++++++++++------ smacc2_dev_tools/.vscode/settings.json | 2 +- 13 files changed, 73 insertions(+), 28 deletions(-) diff --git a/smacc2/include/smacc2/client_bases/smacc_action_client_base.hpp b/smacc2/include/smacc2/client_bases/smacc_action_client_base.hpp index e2b999124..ec1011a23 100644 --- a/smacc2/include/smacc2/client_bases/smacc_action_client_base.hpp +++ b/smacc2/include/smacc2/client_bases/smacc_action_client_base.hpp @@ -285,7 +285,9 @@ class SmaccActionClientBase : public ISmaccActionClient if (resultCallbackPtr != nullptr) { - RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] Result CB calling user callback"); + RCLCPP_INFO_STREAM( + getLogger(), "[" << getName() << "] Result CB calling user callback:" + << demangleSymbol(typeid(*resultCallbackPtr).name())); (*resultCallbackPtr)(result); } else diff --git a/smacc2/include/smacc2/impl/smacc_state_machine_impl.hpp b/smacc2/include/smacc2/impl/smacc_state_machine_impl.hpp index 5543d4ab5..4710a1c6e 100644 --- a/smacc2/include/smacc2/impl/smacc_state_machine_impl.hpp +++ b/smacc2/include/smacc2/impl/smacc_state_machine_impl.hpp @@ -433,6 +433,13 @@ boost::signals2::connection ISmaccStateMachine::createSignalConnection( std::is_base_of::value || std::is_base_of::value) { + RCLCPP_INFO( + getLogger(), + "[StateMachine] long life-time smacc signal subscription created. Subscriber is %s. Callback " + "is: %s", + demangledTypeName().c_str(), + demangleSymbol(typeid(callback).name()).c_str()); + connection = binder.bindaux(signal, callback, object, nullptr); } else if ( diff --git a/smacc2/include/smacc2/smacc_state_machine.hpp b/smacc2/include/smacc2/smacc_state_machine.hpp index 9c61f3bdd..6bdae3a25 100644 --- a/smacc2/include/smacc2/smacc_state_machine.hpp +++ b/smacc2/include/smacc2/smacc_state_machine.hpp @@ -196,6 +196,8 @@ class ISmaccStateMachine // orthogonals std::map> orthogonals_; + std::vector longLivedSignalConnections_; + protected: std::shared_ptr stateMachineInfo_; diff --git a/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.hpp b/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.hpp index aa9c06e28..192d7b3bd 100644 --- a/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.hpp +++ b/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.hpp @@ -109,9 +109,11 @@ class CpWaypointNavigator : public smacc2::ISmaccComponent void removeWaypoint(int index); - void onGoalReached(ClNav2Z::WrappedResult & res); - void onGoalCancelled(ClNav2Z::WrappedResult & /*res*/); - void onGoalAborted(ClNav2Z::WrappedResult & /*res*/); + void onNavigationResult(const ClNav2Z::WrappedResult & r); + + void onGoalReached(const ClNav2Z::WrappedResult & res); + void onGoalCancelled(const ClNav2Z::WrappedResult & /*res*/); + void onGoalAborted(const ClNav2Z::WrappedResult & /*res*/); std::vector waypoints_; diff --git a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_absolute_rotate.cpp b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_absolute_rotate.cpp index 56cacb6d3..4042eb827 100644 --- a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_absolute_rotate.cpp +++ b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_absolute_rotate.cpp @@ -68,7 +68,7 @@ void CbAbsoluteRotate::onEntry() ClNav2Z::Goal goal; goal.pose.header.frame_id = referenceFrame; - goal.pose.header.stamp = getNode()->now(); + //goal.pose.header.stamp = getNode()->now(); auto targetAngle = goal_angle * M_PI / 180.0; goal.pose.pose.position = currentPoseMsg.position; diff --git a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_nav2z_client_behavior_base.cpp b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_nav2z_client_behavior_base.cpp index 6a7abe666..6794e7bf5 100644 --- a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_nav2z_client_behavior_base.cpp +++ b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_nav2z_client_behavior_base.cpp @@ -25,6 +25,8 @@ CbNav2ZClientBehaviorBase::~CbNav2ZClientBehaviorBase() {} void CbNav2ZClientBehaviorBase::sendGoal(ClNav2Z::Goal & goal) { + RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] Creating sending goal callback signal."); + this->navigationCallback_ = std::make_shared(); this->getStateMachine()->createSignalConnection( diff --git a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_navigate_backward.cpp b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_navigate_backward.cpp index 4c599066a..351a7d1e4 100644 --- a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_navigate_backward.cpp +++ b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_navigate_backward.cpp @@ -63,7 +63,7 @@ void CbNavigateBackwards::onEntry() ClNav2Z::Goal goal; goal.pose.header.frame_id = referenceFrame; - goal.pose.header.stamp = getNode()->now(); + //goal.pose.header.stamp = getNode()->now(); tf2::toMsg(targetPose, goal.pose.pose); RCLCPP_INFO_STREAM(getLogger(), "[CbNavigateBackwards] TARGET POSE BACKWARDS: " << goal.pose); diff --git a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_navigate_forward.cpp b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_navigate_forward.cpp index d60928b7a..76d813a4c 100644 --- a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_navigate_forward.cpp +++ b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_navigate_forward.cpp @@ -113,7 +113,7 @@ void CbNavigateForward::onEntry() // action goal ClNav2Z::Goal goal; goal.pose.header.frame_id = referenceFrame; - goal.pose.header.stamp = getNode()->now(); + //goal.pose.header.stamp = getNode()->now(); tf2::toMsg(targetPose, goal.pose.pose); RCLCPP_INFO_STREAM( getLogger(), "[" << getName() << "]" @@ -122,7 +122,9 @@ void CbNavigateForward::onEntry() // current pose geometry_msgs::msg::PoseStamped currentStampedPoseMsg; currentStampedPoseMsg.header.frame_id = referenceFrame; - currentStampedPoseMsg.header.stamp = getNode()->now(); + currentStampedPoseMsg.header.stamp = + getNode()->now(); // probably it is better avoid setting that goal timestamp + tf2::toMsg(currentPose, currentStampedPoseMsg.pose); odomTracker_ = nav2zClient_->getComponent(); diff --git a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_navigate_global_position.cpp b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_navigate_global_position.cpp index 6147f9736..83f79e92d 100644 --- a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_navigate_global_position.cpp +++ b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_navigate_global_position.cpp @@ -89,7 +89,7 @@ void CbNavigateGlobalPosition::execute() RCLCPP_INFO(getLogger(), "Sending Goal to MoveBase"); ClNav2Z::Goal goal; goal.pose.header.frame_id = referenceFrame; - goal.pose.header.stamp = getNode()->now(); + //goal.pose.header.stamp = getNode()->now(); goal.pose.pose.position = goalPosition; tf2::Quaternion q; diff --git a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_rotate.cpp b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_rotate.cpp index 163ec5036..f874067af 100644 --- a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_rotate.cpp +++ b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_rotate.cpp @@ -58,7 +58,7 @@ void CbRotate::onEntry() auto odomTracker = nav2zClient_->getComponent(); ClNav2Z::Goal goal; goal.pose.header.frame_id = referenceFrame; - goal.pose.header.stamp = getNode()->now(); + //goal.pose.header.stamp = getNode()->now(); auto currentAngle = tf2::getYaw(currentPoseMsg.orientation); auto targetAngle = currentAngle + angle_increment_degree * M_PI / 180.0; diff --git a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_undo_path_backwards.cpp b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_undo_path_backwards.cpp index db341d061..b3a767da8 100644 --- a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_undo_path_backwards.cpp +++ b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_undo_path_backwards.cpp @@ -72,11 +72,15 @@ void CbUndoPathBackwards::onEntry() if (forwardpath.poses.size() > 0) { goal.pose = forwardpath.poses.front(); - goal.pose.header.stamp = getNode()->now(); + //goal.pose.header.stamp = getNode()->now(); + goal.pose.header.stamp = rclcpp::Time(0); - if (options_->undoControllerName_) + if (options_ && options_->undoControllerName_) { plannerSwitcher->setUndoPathBackwardPlanner(false); + RCLCPP_INFO_STREAM( + getLogger(), + "[" << getName() << "] Undoing path with controller: " << *options_->undoControllerName_); plannerSwitcher->setDesiredController(*options_->undoControllerName_); plannerSwitcher->commitPublish(); } diff --git a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.cpp b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.cpp index 3ed48aceb..4d0a62f09 100644 --- a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.cpp +++ b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.cpp @@ -38,21 +38,21 @@ CpWaypointNavigator::CpWaypointNavigator() : currentWaypoint_(0), waypoints_(0) void CpWaypointNavigator::onInitialize() { client_ = dynamic_cast(owner_); } -void CpWaypointNavigator::onGoalCancelled(ClNav2Z::WrappedResult & /*res*/) +void CpWaypointNavigator::onGoalCancelled(const ClNav2Z::WrappedResult & /*res*/) { stopWaitingResult(); this->onNavigationRequestCancelled(); } -void CpWaypointNavigator::onGoalAborted(ClNav2Z::WrappedResult & /*res*/) +void CpWaypointNavigator::onGoalAborted(const ClNav2Z::WrappedResult & /*res*/) { stopWaitingResult(); this->onNavigationRequestAborted(); } -void CpWaypointNavigator::onGoalReached(ClNav2Z::WrappedResult & /*res*/) +void CpWaypointNavigator::onGoalReached(const ClNav2Z::WrappedResult & /*res*/) { waypointsEventDispatcher.postWaypointEvent(currentWaypoint_); currentWaypoint_++; @@ -232,15 +232,19 @@ CpWaypointNavigator::sendNextGoal( } // SEND GOAL - if (!succeddedNav2ZClientConnection_.connected()) - { - this->succeddedNav2ZClientConnection_ = - client_->onSucceeded(&CpWaypointNavigator::onGoalReached, this); - this->cancelledNav2ZClientConnection_ = - client_->onAborted(&CpWaypointNavigator::onGoalCancelled, this); - this->abortedNav2ZClientConnection_ = - client_->onCancelled(&CpWaypointNavigator::onGoalAborted, this); - } + // if (!succeddedNav2ZClientConnection_.connected()) + // { + // this->succeddedNav2ZClientConnection_ = + // client_->onSucceeded(&WaypointNavigator::onGoalReached, this); + // this->cancelledNav2ZClientConnection_ = + // client_->onAborted(&WaypointNavigator::onGoalCancelled, this); + // this->abortedNav2ZClientConnection_ = + // client_->onCancelled(&WaypointNavigator::onGoalAborted, this); + // } + + auto callbackptr = resultCallback.lock(); + succeddedNav2ZClientConnection_ = this->getStateMachine()->createSignalConnection( + *callbackptr, &CpWaypointNavigator::onNavigationResult, this); return client_->sendGoal(goal, resultCallback); } @@ -248,12 +252,32 @@ CpWaypointNavigator::sendNextGoal( { RCLCPP_WARN( getLogger(), - "[WaypointsNavigator] All waypoints were consumed. There is no more waypoints available."); + "[CpWaypointsNavigator] All waypoints were consumed. There is no more waypoints available."); } return std::nullopt; } +void CpWaypointNavigator::onNavigationResult(const ClNav2Z::WrappedResult & r) +{ + if (r.code == rclcpp_action::ResultCode::SUCCEEDED) + { + this->onGoalReached(r); + } + else if (r.code == rclcpp_action::ResultCode::ABORTED) + { + this->onGoalAborted(r); + } + else if (r.code == rclcpp_action::ResultCode::CANCELED) + { + this->onGoalCancelled(r); + } + else + { + this->onGoalAborted(r); + } +} + void CpWaypointNavigator::insertWaypoint(int index, geometry_msgs::msg::Pose & newpose) { if (index >= 0 && index <= (int)waypoints_.size()) @@ -361,7 +385,7 @@ void CpWaypointNavigator::loadWayPointsFromFile(std::string filepath) if (wp_node != NULL) { - for (unsigned int i = 0; i < wp_node->size(); ++i) + for (uint64_t i = 0; i < wp_node->size(); ++i) { // Parse waypoint entries on YAML geometry_msgs::msg::Pose wp; @@ -434,7 +458,7 @@ void CpWaypointNavigator::loadWayPointsFromFile2(std::string filepath) if (wp_node != NULL) { - for (unsigned int i = 0; i < wp_node->size(); ++i) + for (uint64_t i = 0; i < wp_node->size(); ++i) { // Parse waypoint entries on YAML geometry_msgs::msg::Pose wp; diff --git a/smacc2_dev_tools/.vscode/settings.json b/smacc2_dev_tools/.vscode/settings.json index 609376462..40677468f 100644 --- a/smacc2_dev_tools/.vscode/settings.json +++ b/smacc2_dev_tools/.vscode/settings.json @@ -121,6 +121,6 @@ "python.analysis.extraPaths": [ "/opt/ros/galactic/lib/python3.8/site-packages" ], - "cmake.sourceDirectory": "${workspaceFolder}/smacc2", + "cmake.sourceDirectory": "/home/geus/Desktop/smacc2_ws/src/SMACC2/.work/target_ws/build/smacc2_msgs/smacc2_msgs__py", } From 797f3c901627f07f67474ed6e2823c83034d2441 Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Sat, 10 Jun 2023 09:15:35 +0000 Subject: [PATCH 02/70] more fixes --- .../sm_dance_bot/launch/bringup_launch.py | 14 +------------- .../sm_dance_bot/launch/gazebo_launch.py | 9 +++++---- 2 files changed, 6 insertions(+), 17 deletions(-) diff --git a/smacc2_sm_reference_library/sm_dance_bot/launch/bringup_launch.py b/smacc2_sm_reference_library/sm_dance_bot/launch/bringup_launch.py index 056c0be50..1714986e4 100644 --- a/smacc2_sm_reference_library/sm_dance_bot/launch/bringup_launch.py +++ b/smacc2_sm_reference_library/sm_dance_bot/launch/bringup_launch.py @@ -57,7 +57,7 @@ def generate_launch_description(): ) declare_slam_cmd = DeclareLaunchArgument( - "slam", default_value="False", description="Whether run a SLAM" + "slam", default_value="true", description="Whether run a SLAM" ) declare_map_yaml_cmd = DeclareLaunchArgument( @@ -100,18 +100,6 @@ def generate_launch_description(): "params_file": params_file, }.items(), ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(launch_dir, "localization_launch.py")), - condition=IfCondition(PythonExpression(["not ", slam])), - launch_arguments={ - "namespace": namespace, - "map": map_yaml_file, - "use_sim_time": use_sim_time, - "autostart": autostart, - "params_file": params_file, - "use_lifecycle_mgr": "false", - }.items(), - ), IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join(launch_dir, "navigation_launch.py")), launch_arguments={ diff --git a/smacc2_sm_reference_library/sm_dance_bot/launch/gazebo_launch.py b/smacc2_sm_reference_library/sm_dance_bot/launch/gazebo_launch.py index 3136af648..30ead3d33 100644 --- a/smacc2_sm_reference_library/sm_dance_bot/launch/gazebo_launch.py +++ b/smacc2_sm_reference_library/sm_dance_bot/launch/gazebo_launch.py @@ -24,7 +24,7 @@ def generate_launch_description(): declare_use_simulator_cmd = DeclareLaunchArgument( - "use_simulator", default_value="False", description="Whether to execute gzclient)" + "use_simulator", default_value="false", description="Whether to execute gzclient)" ) use_simulator = LaunchConfiguration("use_simulator") @@ -36,11 +36,11 @@ def generate_launch_description(): launch_dir = os.path.join(sm_dance_bot_dir, "launch") declare_use_simulator_cmd = DeclareLaunchArgument( - "use_simulator", default_value="True", description="Whether to start the simulator" + "use_simulator", default_value="true", description="Whether to start the simulator" ) declare_simulator_cmd = DeclareLaunchArgument( - "headless", default_value="False", description="Whether to execute gzclient)" + "headless", default_value="false", description="Whether to execute gzclient)" ) declare_show_gz_lidar = DeclareLaunchArgument( @@ -83,7 +83,8 @@ def generate_launch_description(): ) start_gazebo_client_cmd = ExecuteProcess( - condition=IfCondition(PythonExpression([use_simulator, " and not ", headless])), + # condition=IfCondition(PythonExpression([use_simulator, " and not ", headless])), + condition=UnlessCondition(headless), cmd=["gzclient"], cwd=[launch_dir], env=gzenv, From 44b432a81e2c1b0e08285ba796d00e7a346224b9 Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Sat, 10 Jun 2023 12:59:21 +0000 Subject: [PATCH 03/70] tests --- .../sm_dancebot_ue/CMakeLists.txt | 99 +++++++++++++++ .../sm_dancebot_ue/docker/Dockerfile | 115 ++++++++++++++++++ .../sm_dancebot_ue/docker/build_docker.sh | 21 ++++ .../docker/build_docker_humble.sh | 4 + .../sm_dancebot_ue/docker/fastdds_setup.bash | 5 + .../sm_dancebot_ue/docker/join_bash.sh | 1 + .../docker/run_docker_bash_humble.sh | 7 ++ .../sm_dancebot_ue/docker/run_editor_smacc.sh | 3 + 8 files changed, 255 insertions(+) create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/CMakeLists.txt create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile create mode 100755 smacc2_sm_reference_library/sm_dancebot_ue/docker/build_docker.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_ue/docker/build_docker_humble.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_ue/docker/fastdds_setup.bash create mode 100755 smacc2_sm_reference_library/sm_dancebot_ue/docker/join_bash.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_bash_humble.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_ue/docker/run_editor_smacc.sh diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/CMakeLists.txt b/smacc2_sm_reference_library/sm_dancebot_ue/CMakeLists.txt new file mode 100644 index 000000000..5ae4e6577 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/CMakeLists.txt @@ -0,0 +1,99 @@ +cmake_minimum_required(VERSION 3.5) +project(sm_dancebot_ue) + +# 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(ament_cmake_python REQUIRED) +find_package(Boost COMPONENTS thread REQUIRED) + +find_package(smacc2 REQUIRED) +find_package(std_msgs REQUIRED) +find_package(nav2z_client REQUIRED) +find_package(ros_timer_client REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) + +set(dependencies + smacc2 + std_msgs + sensor_msgs + nav2z_client + ros_timer_client + visualization_msgs +) + +# rosidl_generate_interfaces(${PROJECT_NAME} +# "servers/led_action_server/action/LEDControl.action" +# DEPENDENCIES builtin_interfaces std_msgs action_msgs sensor_msgs +# ) + +include_directories(include + ${Boost_INCLUDE_DIRS} + ${CMAKE_BINARY_DIR}/rosidl_generator_cpp) + +add_executable(${PROJECT_NAME}_node + src/${PROJECT_NAME}/sm_dancebot_ue_node.cpp + # src/${PROJECT_NAME}/clients/cl_led/cl_led.cpp +) + +# add_executable(temperature_sensor_node_${PROJECT_NAME} servers/temperature_sensor_node/src/temperature_sensor_node.cpp) +# add_executable(led_action_server_node_${PROJECT_NAME} servers/led_action_server/src/led_action_server_node.cpp) + +ament_target_dependencies(${PROJECT_NAME}_node ${dependencies}) +#target_link_libraries(${PROJECT_NAME}_node ${Boost_LIBS} ${PROJECT_NAME}__rosidl_typesupport_cpp) + +# ament_target_dependencies(led_action_server_node_${PROJECT_NAME} ${dependencies}) +# set_target_properties(led_action_server_node_${PROJECT_NAME} PROPERTIES OUTPUT_NAME "led_action_server_node") +# target_link_libraries(led_action_server_node_${PROJECT_NAME} ${Boost_LIBS} ${PROJECT_NAME}__rosidl_typesupport_cpp) + +# ament_target_dependencies(temperature_sensor_node_${PROJECT_NAME} ${dependencies}) +# set_target_properties(temperature_sensor_node_${PROJECT_NAME} PROPERTIES OUTPUT_NAME "temperature_sensor_node") + +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) +ament_export_dependencies(${dependencies}) + +# ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/env-hooks/sm_dancebot_ue.dsv.in") + +if(BUILD_TESTING) +endif() + +install(TARGETS + ${PROJECT_NAME}_node + # temperature_sensor_node_${PROJECT_NAME} + # led_action_server_node_${PROJECT_NAME} + + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +#ament_python_install_package(${PROJECT_NAME} +# PACKAGE_DIR servers/service_node_3 +# ) + + +install(FILES + # servers/service_node_3/service_node_3.py + DESTINATION + lib/${PROJECT_NAME} + PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READ + GROUP_EXECUTE GROUP_READ) + +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +#install(DIRECTORY worlds DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY params DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY urdf DESTINATION share/${PROJECT_NAME}) +#install(DIRECTORY models DESTINATION share/${PROJECT_NAME}) + +ament_package() diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile b/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile new file mode 100644 index 000000000..a6b387082 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile @@ -0,0 +1,115 @@ +ARG ROS_DISTRO=rolling +ARG GIT_BRANCH=master +ARG UBUNTU_VERSION=focal + +FROM ros:$ROS_DISTRO-ros-base-$UBUNTU_VERSION + +ARG ROS_DISTRO +ARG GIT_BRANCH +ARG LOCAL_FOLDER_SOURCE=1 +ENV DEBIAN_FRONTEND=noninteractive + +RUN apt-get update \ + && apt-get upgrade -y \ + && apt-get clean + +RUN apt-get update && apt-get install -q -y --no-install-recommends \ + dirmngr \ + gnupg2 \ + lsb-release \ + python3-colcon-ros \ + && apt-get clean \ + && apt upgrade -y --with-new-pkgs + +RUN apt-get install -y nvidia-driver-525 + +RUN apt install -y xdg-user-dirs + +ENV USER=ros2_ws +RUN bash -c "useradd -ms /bin/bash $USER" +RUN usermod -aG sudo $USER +RUN echo '%sudo ALL=(ALL) NOPASSWD:ALL' >> /etc/sudoers +RUN chown -R $USER:$USER /home/$USER + +USER $USER +WORKDIR /home/$USER + +WORKDIR "/home/$USER/src" + +RUN echo "copying current code version to docker image:" +#ADD . /home/$USER/src/SMACC2 +#WORKDIR "/home/$USER" + +# install dependencies and build +# RUN vcs import src --skip-existing --input src/SMACC2/.github/SMACC2-not-released.$ROS_DISTRO.repos +# RUN ls src + +# RUN apt update +# RUN rosdep install --from-paths src --ignore-src -r -y +# RUN apt-get update && apt-get install -q -y --no-install-recommends xterm + +# RUN bash -c "source /opt/ros/$ROS_DISTRO/setup.sh && colcon build --merge-install --parallel-workers 1" + +COPY --chown=$USER:$USER smacc2_sm_reference_library/sm_dancebot_ue/docker/UE5.1/UnrealEngine /home/$USER/src/UE5.1/UnrealEngine + +RUN sudo apt-get install -y wget gpg +RUN wget -qO- https://packages.microsoft.com/keys/microsoft.asc | sudo gpg --dearmor > packages.microsoft.gpg +RUN sudo install -D -o root -g root -m 644 packages.microsoft.gpg /etc/apt/keyrings/packages.microsoft.gpg +RUN sudo sh -c 'echo "deb [arch=amd64,arm64,armhf signed-by=/etc/apt/keyrings/packages.microsoft.gpg] https://packages.microsoft.com/repos/code stable main" > /etc/apt/sources.list.d/vscode.list' -f packages.microsoft.gpg + +RUN sudo apt install -y apt-transport-https +RUN sudo apt update +RUN sudo apt install -y code git-lfs # or code-insiders + +RUN git clone --recurse-submodules https://github.com/rapyuta-robotics/turtlebot3-UE.git -b jammy +WORKDIR "/home/$USER/src/turtlebot3-UE" +RUN git-lfs pull && git submodule foreach git-lfs pull +ENV UE5_DIR=/home/$USER/src/UE5.1/UnrealEngine +RUN ./update_project_files.sh +ENV UE5_DIR=/home/$USER/src/UE5.1 +RUN sudo apt-get install nano + +RUN wget http://security.ubuntu.com/ubuntu/pool/universe/t/tinyxml2/libtinyxml2-6a_7.0.0+dfsg-1build1_amd64.deb +RUN wget http://archive.ubuntu.com/ubuntu/pool/main/o/openssl/libssl1.1_1.1.0g-2ubuntu4_amd64.deb +RUN sudo dpkg -i libtinyxml2-6a_7.0.0+dfsg-1build1_amd64.deb +RUN sudo dpkg -i libssl1.1_1.1.0g-2ubuntu4_amd64.deb +ENV LD_LIBRARY_PATH=/usr/local/cuda/lib64:/usr/lib/x86_64-linux-gnu/gazebo-11/plugins:/opt/ros/humble/opt/rviz_ogre_vendor/lib:/opt/ros/humble/lib/x86_64-linux-gnu:/home/$USER/src/turtlebot3-UE/Plugins/rclUE/ThirdParty/ros/lib/ +ENV UE5_DIR=/home/$USER/src/UE5.1/UnrealEngine + +RUN make turtlebot3 +RUN make turtlebot3Editor + + +# COPY smacc2 /home/$USER/src/SMACC2/smacc2 +RUN mkdir -p /home/$USER/src/SMACC2/ +WORKDIR "/home/$USER/src/SMACC2" +COPY --chown=$USER:$USER smacc2_client_library /home/$USER/src/SMACC2/smacc2_client_library +COPY --chown=$USER:$USER smacc2_event_generator_library /home/$USER/src/SMACC2/smacc2_event_generator_library +COPY --chown=$USER:$USER smacc2_performance_tools /home/$USER/src/SMACC2/smacc2_performance_tools +COPY --chown=$USER:$USER smacc2_state_reactor_library /home/$USER/src/SMACC2/smacc2_state_reactor_library +COPY --chown=$USER:$USER smacc2 /home/$USER/src/SMACC2/smacc2 +COPY --chown=$USER:$USER smacc2_dev_tools /home/$USER/src/SMACC2/smacc2_dev_tools +COPY --chown=$USER:$USER smacc2_msgs /home/$USER/src/SMACC2/smacc2_msgs +#smacc2_sm_reference_library + +# ADD smacc2_client_library /home/$USER/src/SMACC2_ws/src/SMACC2/smacc2_client_library +# ADD smacc2_state_reactor_library /home/$USER/src/SMACC2_ws/src/SMACC2/smacc2_state_reactor_library +# ADD smacc2_event_generator_library /home/$USER/src/SMACC2_ws/src/SMACC2/smacc2_event_generator_library +#RUN sudo chown -R $USER:$USER /home/$USER + +#RUN git clone +RUN rosdep update +RUN rosdep install --ignore-src --from-paths . -y -r + +COPY --chown=$USER:$USER smacc2_sm_reference_library/sm_dancebot_ue/docker/run_editor_smacc.sh /home/$USER/src/turtlebot3-UE/run_editor_smacc.sh + +RUN touch /home/$USER/src/turtlebot3-UE/COLCON_IGNORE +RUN touch /home/$USER/src/UE5.1/COLCON_IGNORE +WORKDIR "/home/$USER/" +RUN bash -c "source /opt/ros/humble/setup.bash && colcon build --symlink-install" + +WORKDIR "/home/$USER/src/turtlebot3-UE" +ENTRYPOINT /home/$USER/src/turtlebot3-UE/run_editor_smacc.sh + + + diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/build_docker.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/build_docker.sh new file mode 100755 index 000000000..518fcd307 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/build_docker.sh @@ -0,0 +1,21 @@ +#!/bin/sh +DIR="$(dirname "$(realpath "$0")")" +echo $DIR +cd $DIR/.. +echo `pwd` + +ROS_DISTRO=$1 +GIT_BRANCH=$2 +UBUNTU_VERSION=$3 +NOCACHE="--no-cache" +NOCACHE= + +ROOT_DIR=`realpath $DIR/../../..` + +echo "ros distro: $ROS_DISTRO" +echo "git branch: $GIT_BRANCH" +echo "ubuntu version: $UBUNTU_VERSION" +echo "root path: $ROOT_DIR" + +cd $ROOT_DIR +sudo docker build --build-arg ROS_DISTRO=$ROS_DISTRO --build-arg GIT_BRANCH=$GIT_BRANCH --build-arg UBUNTU_VERSION=$UBUNTU_VERSION -t ue_editor_rclue:$ROS_DISTRO --progress string -f $ROOT_DIR/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile . $NOCACHE diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/build_docker_humble.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/build_docker_humble.sh new file mode 100755 index 000000000..ed0429b57 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/build_docker_humble.sh @@ -0,0 +1,4 @@ +#!/bin/sh +DIR="$(dirname "$(realpath "$0")")" +echo $DIR +$DIR/build_docker.sh humble humble jammy diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/fastdds_setup.bash b/smacc2_sm_reference_library/sm_dancebot_ue/docker/fastdds_setup.bash new file mode 100755 index 000000000..f661c4240 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/fastdds_setup.bash @@ -0,0 +1,5 @@ +#!/bin/bash + +export RMW_IMPLEMENTATION=rmw_fastrtps_cpp +export ROS_DISCOVERY_SERVER="127.0.0.1:11811" +export FASTRTPS_DEFAULT_PROFILES_FILE=${PWD}/fastdds_config.xml diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/join_bash.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/join_bash.sh new file mode 100755 index 000000000..0f043d36d --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/join_bash.sh @@ -0,0 +1 @@ +sudo docker exec -it $(sudo docker ps -aqf "ancestor=ue_editor_rclue:humble"| head -n 1) /bin/bash diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_bash_humble.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_bash_humble.sh new file mode 100755 index 000000000..827fe76c8 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_bash_humble.sh @@ -0,0 +1,7 @@ +#!/bin/sh +xhost + + +DIR="$(dirname "$(realpath "$0")")" +ROOT_DIR=`realpath $DIR/../../..` + +sudo nvidia-docker run -it -e DISPLAY --network bridge -e QT_X11_NO_MITSHM=1 --privileged -v $ROOT_DIR:/home/ros2_ws/src/SMACC2 -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_editor_smacc.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_editor_smacc.sh new file mode 100755 index 000000000..0e86cda47 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_editor_smacc.sh @@ -0,0 +1,3 @@ +#!/bin/sh +source fastdds_setup.sh +./run_editor.sh \ No newline at end of file From 5107a169c262d2f6a28625938480628cd748b92e Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Sat, 10 Jun 2023 18:02:37 +0000 Subject: [PATCH 04/70] changes smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.cpp --- .../sm_dancebot_ue/CHANGELOG.rst | 13067 ++++++++++++++++ .../sm_dancebot_ue/CMakeLists.txt | 49 +- .../sm_dancebot_ue/README.md | 55 + .../sm_dancebot_ue/clients/cl_led/cl_led.hpp | 44 + .../cl_led/client_behaviors/cb_led_off.hpp | 50 + .../cl_led/client_behaviors/cb_led_on.hpp | 50 + .../clients/cl_lidar/cl_lidar.hpp | 38 + .../client_behaviors/cb_lidar_sensor.hpp | 45 + .../clients/cl_service3/cl_service3.hpp | 35 + .../client_behaviors/cb_service3.hpp | 57 + .../cl_string_publisher.hpp | 40 + .../client_behaviors/cb_string_publisher.hpp | 55 + .../cl_temperature_sensor.hpp | 38 + ...cb_custom_condition_temperature_sensor.hpp | 56 + .../modestates/ms_dance_bot_recovery_mode.hpp | 38 + .../modestates/ms_dance_bot_run_mode.hpp | 37 + .../sm_dancebot_ue/orthogonals/or_led.hpp | 35 + .../orthogonals/or_navigation.hpp | 88 + .../orthogonals/or_obstacle_perception.hpp | 40 + .../orthogonals/or_service3.hpp | 37 + .../orthogonals/or_string_publisher.hpp | 32 + .../orthogonals/or_temperature_sensor.hpp | 43 + .../sm_dancebot_ue/orthogonals/or_timer.hpp | 36 + .../orthogonals/or_updatable_publisher.hpp | 37 + .../include/sm_dancebot_ue/sm_dancebot_ue.hpp | 198 + .../sti_fpattern_forward_1.hpp | 58 + .../sti_fpattern_forward_2.hpp | 52 + .../sti_fpattern_loop_start.hpp | 55 + .../sti_fpattern_return_1.hpp | 51 + .../sti_fpattern_rotate_1.hpp | 61 + .../sti_fpattern_rotate_2.hpp | 63 + .../sti_radial_end_point.hpp | 50 + .../sti_radial_loop_start.hpp | 59 + .../sti_radial_return.hpp | 56 + .../sti_radial_rotate.hpp | 58 + .../sti_spattern_forward_1.hpp | 49 + .../sti_spattern_forward_2.hpp | 49 + .../sti_spattern_forward_3.hpp | 47 + .../sti_spattern_forward_4.hpp | 51 + .../sti_spattern_loop_start.hpp | 60 + .../sti_spattern_rotate_1.hpp | 61 + .../sti_spattern_rotate_2.hpp | 62 + .../sti_spattern_rotate_3.hpp | 73 + .../sti_spattern_rotate_4.hpp | 61 + .../states/st_acquire_sensors.hpp | 65 + .../states/st_event_count_down.hpp | 58 + .../states/st_navigate_forward_1.hpp | 70 + .../states/st_navigate_forward_2.hpp | 39 + .../states/st_navigate_reverse_1.hpp | 49 + .../states/st_navigate_reverse_2.hpp | 49 + .../states/st_navigate_reverse_3.hpp | 49 + .../states/st_navigate_reverse_4.hpp | 49 + .../states/st_navigate_to_waypoint_1.hpp | 49 + .../states/st_navigate_to_waypoints_x.hpp | 72 + .../states/st_rotate_degrees_1.hpp | 49 + .../states/st_rotate_degrees_2.hpp | 48 + .../states/st_rotate_degrees_3.hpp | 49 + .../states/st_rotate_degrees_4.hpp | 48 + .../states/st_rotate_degrees_5.hpp | 48 + .../states/st_rotate_degrees_6.hpp | 48 + .../superstates/ss_f_pattern_1.hpp | 92 + .../superstates/ss_radial_pattern_1.hpp | 78 + .../superstates/ss_radial_pattern_2.hpp | 81 + .../superstates/ss_radial_pattern_3.hpp | 76 + .../superstates/ss_s_pattern_1.hpp | 93 + .../sm_dancebot_ue/launch/bringup_launch.py | 137 + .../launch/husky_gazebo.launch.yaml | 3 + .../launch/navigation_launch.py | 169 + .../launch/online_sync_launch.py | 48 + .../sm_dancebot_ue/launch/rviz_launch.py | 122 + .../sm_dancebot_ue/launch/slam_launch.py | 128 + .../launch/sm_dance_bot_launch.py | 252 + .../sm_dancebot_ue/package.xml | 44 + .../params/mapper_params_online_sync.yaml | 73 + .../params/nav2z_client/nav2_params.yaml | 367 + .../params/nav2z_client/navigation_tree.xml | 33 + .../params/nav2z_client/waypoints_plan.yaml | 82 + .../nav2z_client_husky/nav2_params.yaml | 367 + .../nav2z_client_husky/navigation_tree.xml | 27 + .../nav2z_client_husky/waypoints_plan.yaml | 82 + .../nav2z_client_turtlebot/nav2_params.yaml | 367 + .../navigation_tree.xml | 27 + .../waypoints_plan.yaml | 82 + .../params/sm_dance_bot_config.yaml | 9 + .../rviz/nav2_default_view.rviz | 811 + .../rviz/nav2_namespaced_view.rviz | 371 + .../action/LEDControl.action | 18 + .../src/led_action_server_node.cpp | 230 + .../servers/service_node_3/service_node_3.py | 47 + .../src/temperature_sensor_node.cpp | 36 + .../sm_dancebot_ue/clients/cl_led/cl_led.cpp | 46 + .../src/sm_dancebot_ue/sm_dancebot_ue.cpp | 27 + .../urdf/turtlebot3_waffle.urdf | 293 + .../worlds/ridgeback_race.world | 4864 ++++++ .../worlds/ridgeback_race_empty.world | 4193 +++++ .../worlds/ridgeback_race_no_lidar.world | 4881 ++++++ 96 files changed, 34851 insertions(+), 20 deletions(-) create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/CHANGELOG.rst create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/README.md create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_led/cl_led.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_led/client_behaviors/cb_led_off.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_led/client_behaviors/cb_led_on.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_lidar/cl_lidar.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_lidar/client_behaviors/cb_lidar_sensor.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_service3/cl_service3.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_service3/client_behaviors/cb_service3.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_string_publisher/cl_string_publisher.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_string_publisher/client_behaviors/cb_string_publisher.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_temperature_sensor/cl_temperature_sensor.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_temperature_sensor/client_behaviors/cb_custom_condition_temperature_sensor.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/modestates/ms_dance_bot_recovery_mode.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/modestates/ms_dance_bot_run_mode.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_led.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_navigation.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_obstacle_perception.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_service3.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_string_publisher.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_temperature_sensor.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_timer.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_updatable_publisher.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/sm_dancebot_ue.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_forward_1.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_forward_2.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_loop_start.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_return_1.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_rotate_1.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_rotate_2.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/radial_motion_states/sti_radial_end_point.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/radial_motion_states/sti_radial_loop_start.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/radial_motion_states/sti_radial_return.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/radial_motion_states/sti_radial_rotate.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_forward_1.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_forward_2.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_forward_3.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_forward_4.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_loop_start.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_rotate_1.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_rotate_2.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_rotate_3.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_rotate_4.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_acquire_sensors.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_event_count_down.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_forward_1.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_forward_2.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_reverse_1.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_reverse_2.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_reverse_3.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_reverse_4.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_to_waypoint_1.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_to_waypoints_x.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_rotate_degrees_1.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_rotate_degrees_2.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_rotate_degrees_3.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_rotate_degrees_4.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_rotate_degrees_5.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_rotate_degrees_6.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_f_pattern_1.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_radial_pattern_1.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_radial_pattern_2.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_radial_pattern_3.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_s_pattern_1.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/launch/bringup_launch.py create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/launch/husky_gazebo.launch.yaml create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/launch/navigation_launch.py create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/launch/online_sync_launch.py create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/launch/rviz_launch.py create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/launch/slam_launch.py create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/launch/sm_dance_bot_launch.py create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/package.xml create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/params/mapper_params_online_sync.yaml create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/nav2_params.yaml create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/navigation_tree.xml create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan.yaml create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_husky/nav2_params.yaml create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_husky/navigation_tree.xml create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_husky/waypoints_plan.yaml create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_turtlebot/nav2_params.yaml create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_turtlebot/navigation_tree.xml create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_turtlebot/waypoints_plan.yaml create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/params/sm_dance_bot_config.yaml create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/rviz/nav2_default_view.rviz create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/rviz/nav2_namespaced_view.rviz create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/servers/led_action_server/action/LEDControl.action create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/servers/led_action_server/src/led_action_server_node.cpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/servers/service_node_3/service_node_3.py create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/servers/temperature_sensor_node/src/temperature_sensor_node.cpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/clients/cl_led/cl_led.cpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/sm_dancebot_ue.cpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/urdf/turtlebot3_waffle.urdf create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/worlds/ridgeback_race.world create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/worlds/ridgeback_race_empty.world create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/worlds/ridgeback_race_no_lidar.world diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/CHANGELOG.rst b/smacc2_sm_reference_library/sm_dancebot_ue/CHANGELOG.rst new file mode 100644 index 000000000..89f28db94 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/CHANGELOG.rst @@ -0,0 +1,13067 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package sm_dance_bot +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.3.6 (2023-03-12) +------------------ + +1.22.1 (2022-11-09) +------------------- +* pre-release +* Contributors: pabloinigoblasco + +* publisher +* Feature/warehouse 3 improvements (#313) + * improvements in navigation client behaviors and husky barrel demo + * many improvements in action client and cb sequence for hysky barrel search + * more and better navigation behaviors on husky barrel search demo + * functionality improvements in navigation and improvements of warehouse 3, format + * functionality improvements in navigation and improvements of warehouse 3 and husky + * format + * warehouse 3 improvements + * merge galactic + * merge fix + * minor +* improvements in navigation client behaviors and husky barrel demo (#311) + * improvements in navigation client behaviors and husky barrel demo + * many improvements in action client and cb sequence for hysky barrel search + * more and better navigation behaviors on husky barrel search demo + * functionality improvements in navigation and improvements of warehouse 3, format + * functionality improvements in navigation and improvements of warehouse 3 and husky + * format +* Feature/galactic rolling merge (#288) + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dance_bot visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dance_bot (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * more refinement in sm_dance_bot + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dance_bot + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dance_bot + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dance_bot tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nav2z renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dance_bot + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * Update cb_navigate_global_position.hpp + * Merging code from backport foxy and updates about autoware (#208) + * minor changes + * replanning for all our examples + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dance_bot visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dance_bot (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * more refinement in sm_dance_bot + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dance_bot + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dance_bot + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dance_bot tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nav2z renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dance_bot + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + * Add mergify rules file. + * Try fixing CI for rolling. (#209) + Merging to get backport working. + * Remove example things from Foxy CI setup. (#214) + * Add Autoware Auto Msgs into not-released dependencies. (#220) + * Fix rolling builds (#222) + * do not merge yet - Feature/odom tracker improvements and retry motion (#223) + * odom tracker improvements + * adding forward behavior retry funcionality + * removing warnings (#213) + * minor changes + * replanning for all our examples + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dance_bot visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dance_bot (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * more refinement in sm_dance_bot + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dance_bot + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dance_bot + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dance_bot tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nav2z renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dance_bot + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + * some reordering fixes + * minor + * docker files for different revisions, warnings removval and more testing on navigation + * fixing docker for foxy and galactic + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + * dockerfiles (#225) + * Fix code generators (#221) + * Fix other build issues. + * Update SM template and make example code clearly visible. + * Remove use of node in the sm performance template. + * Updated templated to use Blackboard storage. + * Update template to resolve the global data correctly. + * Update sm_name.hpp + Co-authored-by: Pablo Iñigo Blasco + * Feature/retry behavior warehouse 1 (#226) + * minor changes + * replanning for all our examples + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dance_bot visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dance_bot (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * more refinement in sm_dance_bot + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dance_bot + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dance_bot + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dance_bot tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nav2z renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dance_bot + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + * some reordering fixes + * minor + * docker files for different revisions, warnings removval and more testing on navigation + * fixing docker for foxy and galactic + * Update file for fake hardware simulation and add file for gazebo simulation. + * docker build files for all versions + * retry behavior warehouse 1 + * missing file + * minor format fix + * other minor changes + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + * fix broken source build (#227) + * Only rolling version should be pre-released on on master. (#230) + * Correct Focal-Rolling builds by fixing the version of rosdep yaml (#234) + * Update file for fake hardware simulation and add file for gazebo simulation. (#224) + * Update file for fake hardware simulation and add file for gazebo simulation. + * Add ignition file and update repos files. + * Feature/improvements warehouse3 (#228) + * minor changes + * replanning for all our examples + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dance_bot visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dance_bot (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * more refinement in sm_dance_bot + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dance_bot + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dance_bot + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dance_bot tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nav2z renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dance_bot + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + * some reordering fixes + * minor + * docker files for different revisions, warnings removval and more testing on navigation + * fixing docker for foxy and galactic + * docker build files for all versions + * barrel demo + * barrel search build fix and warehouse3 + * fixing startup problems in warehouse 3 + * fixing format and minor + * minor + * progress in barrel husky + * minor + * barrel demo + * progress + * fixing broken build + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + * Feature/barrel - do not merge yet (#233) + * minor changes + * replanning for all our examples + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dance_bot visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dance_bot (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * more refinement in sm_dance_bot + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dance_bot + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dance_bot + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dance_bot tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nav2z renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dance_bot + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + * some reordering fixes + * minor + * docker files for different revisions, warnings removval and more testing on navigation + * fixing docker for foxy and galactic + * docker build files for all versions + * barrel demo + * barrel search build fix and warehouse3 + * fixing startup problems in warehouse 3 + * fixing format and minor + * minor + * progress in barrel husky + * minor + * barrel demo + * minor + * barrel search updates + * making models local + * red picuup + * multiple controllable leds plugin + * progress in husky demo + * progressing in husky demo + * improving navigation behaviors + * more merge + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + * Feature/docker improvements march 2022 (#235) + * minor changes + * replanning for all our examples + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dance_bot visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dance_bot (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * more refinement in sm_dance_bot + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dance_bot + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dance_bot + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dance_bot tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nav2z renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dance_bot + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + * some reordering fixes + * minor + * docker files for different revisions, warnings removval and more testing on navigation + * fixing docker for foxy and galactic + * docker build files for all versions + * barrel demo + * barrel search build fix and warehouse3 + * fixing startup problems in warehouse 3 + * fixing format and minor + * minor + * progress in barrel husky + * minor + * barrel demo + * minor + * barrel search updates + * making models local + * red picuup + * multiple controllable leds plugin + * progress in husky demo + * progressing in husky demo + * improving navigation behaviors + * more merge + * docker improvements + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + * Use correct upstream .repos files for source builds (#243) + * Correct mergify branch names (#246) + * Correct name of source-build job and bump version of action (#242) (#247) + Co-authored-by: Denis Štogl + Co-authored-by: Pablo Iñigo Blasco + * Update galactic source build job name (#250) + * Galactic source build: update .repos file, bump action version and use correct version of upstream packages (backport #241) (#248) + Co-authored-by: Denis Štogl + * fixing rolling build (#239) + * fixing rolling build + * trying to fix dependencies + * missing repo + * fixing to focal by the moment + * more fixing rolling build + * minor + * cache matrix rolling and source build package + * minor + * minor + * missing repo + * missing deps + * fixing building issue + * typo + * fixing broken build + * build fix + * restoring workflow files (#252) + * restoring files (#253) + * Fix checkout branches for scheduled builds (#254) + * correct checkout branch on scheduled build + * Update foxy-source-build.yml + * Feature/fixing husky build rolling (#257) + * restoring files + * making husky project build on rolling + * Feature/fixing husky build rolling (#258) + * restoring files + * making husky project build on rolling + * husky progress + * Update README.md (#262) + * Feature/fixing ur demos (#261) + * restoring files + * fixes + * Feature/fixing type string walker (#263) + * restoring files + * fixing type string walker threesome demo + * Update README.md (#266) + * Update README.md (#267) + * Update README.md (#268) + * Significant update in Getting Started Instructions (#269) + * Significant update in Getting Started Instructions + * Remove trailing spaces. + Co-authored-by: Denis Štogl + * fixing ur demo (#273) + * fix: initialise conditionFlag as false (#274) + * precommit fix (#280) + I merge this in red for focal-rolling because it was already broken anyway and it is only a minor update of the precommit + * Ignore packages which should not be released. + * Added changelogs. + * 0.4.0 + * Revert "Ignore packages which should not be released." + This reverts commit ee2cc86db3c0a24f9eb0a9e33217de3f7a691a1c. + * Fix urls to index.ros.org (#284) + * Fix foxy source build config to use repos file from foxy branch. (#285) + * adding spawn entity delays + * more on backport + * more on backport + * disappeared ur_msgs denis repo + * fixing sm_dance_bot examples + * working on fix of image messages for husky_barrel demo + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + Co-authored-by: mergify[bot] <37929162+mergify[bot]@users.noreply.github.com> + Co-authored-by: brettpac +* Revert "Ignore packages which should not be released." + This reverts commit dec14a936a877b2ef722a6a32f1bf3df09312542. +* Contributors: Denis Štogl, Pablo Iñigo Blasco, pabloinigoblasco + +0.3.0 (2022-04-04) +------------------ + +0.0.0 (2022-11-09) +------------------ +* publisher +* Feature/warehouse 3 improvements (#313) + * improvements in navigation client behaviors and husky barrel demo + * many improvements in action client and cb sequence for hysky barrel search + * more and better navigation behaviors on husky barrel search demo + * functionality improvements in navigation and improvements of warehouse 3, format + * functionality improvements in navigation and improvements of warehouse 3 and husky + * format + * warehouse 3 improvements + * merge galactic + * merge fix + * minor +* improvements in navigation client behaviors and husky barrel demo (#311) + * improvements in navigation client behaviors and husky barrel demo + * many improvements in action client and cb sequence for hysky barrel search + * more and better navigation behaviors on husky barrel search demo + * functionality improvements in navigation and improvements of warehouse 3, format + * functionality improvements in navigation and improvements of warehouse 3 and husky + * format +* Feature/galactic rolling merge (#288) + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dance_bot visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dance_bot (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * more refinement in sm_dance_bot + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dance_bot + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dance_bot + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dance_bot tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nav2z renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dance_bot + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * Update cb_navigate_global_position.hpp + * Merging code from backport foxy and updates about autoware (#208) + * minor changes + * replanning for all our examples + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dance_bot visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dance_bot (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * more refinement in sm_dance_bot + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dance_bot + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dance_bot + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dance_bot tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nav2z renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dance_bot + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + * Add mergify rules file. + * Try fixing CI for rolling. (#209) + Merging to get backport working. + * Remove example things from Foxy CI setup. (#214) + * Add Autoware Auto Msgs into not-released dependencies. (#220) + * Fix rolling builds (#222) + * do not merge yet - Feature/odom tracker improvements and retry motion (#223) + * odom tracker improvements + * adding forward behavior retry funcionality + * removing warnings (#213) + * minor changes + * replanning for all our examples + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dance_bot visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dance_bot (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * more refinement in sm_dance_bot + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dance_bot + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dance_bot + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dance_bot tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nav2z renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dance_bot + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + * some reordering fixes + * minor + * docker files for different revisions, warnings removval and more testing on navigation + * fixing docker for foxy and galactic + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + * dockerfiles (#225) + * Fix code generators (#221) + * Fix other build issues. + * Update SM template and make example code clearly visible. + * Remove use of node in the sm performance template. + * Updated templated to use Blackboard storage. + * Update template to resolve the global data correctly. + * Update sm_name.hpp + Co-authored-by: Pablo Iñigo Blasco + * Feature/retry behavior warehouse 1 (#226) + * minor changes + * replanning for all our examples + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dance_bot visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dance_bot (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * more refinement in sm_dance_bot + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dance_bot + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dance_bot + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dance_bot tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nav2z renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dance_bot + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + * some reordering fixes + * minor + * docker files for different revisions, warnings removval and more testing on navigation + * fixing docker for foxy and galactic + * Update file for fake hardware simulation and add file for gazebo simulation. + * docker build files for all versions + * retry behavior warehouse 1 + * missing file + * minor format fix + * other minor changes + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + * fix broken source build (#227) + * Only rolling version should be pre-released on on master. (#230) + * Correct Focal-Rolling builds by fixing the version of rosdep yaml (#234) + * Update file for fake hardware simulation and add file for gazebo simulation. (#224) + * Update file for fake hardware simulation and add file for gazebo simulation. + * Add ignition file and update repos files. + * Feature/improvements warehouse3 (#228) + * minor changes + * replanning for all our examples + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dance_bot visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dance_bot (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * more refinement in sm_dance_bot + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dance_bot + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dance_bot + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dance_bot tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nav2z renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dance_bot + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + * some reordering fixes + * minor + * docker files for different revisions, warnings removval and more testing on navigation + * fixing docker for foxy and galactic + * docker build files for all versions + * barrel demo + * barrel search build fix and warehouse3 + * fixing startup problems in warehouse 3 + * fixing format and minor + * minor + * progress in barrel husky + * minor + * barrel demo + * progress + * fixing broken build + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + * Feature/barrel - do not merge yet (#233) + * minor changes + * replanning for all our examples + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dance_bot visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dance_bot (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * more refinement in sm_dance_bot + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dance_bot + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dance_bot + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dance_bot tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nav2z renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dance_bot + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + * some reordering fixes + * minor + * docker files for different revisions, warnings removval and more testing on navigation + * fixing docker for foxy and galactic + * docker build files for all versions + * barrel demo + * barrel search build fix and warehouse3 + * fixing startup problems in warehouse 3 + * fixing format and minor + * minor + * progress in barrel husky + * minor + * barrel demo + * minor + * barrel search updates + * making models local + * red picuup + * multiple controllable leds plugin + * progress in husky demo + * progressing in husky demo + * improving navigation behaviors + * more merge + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + * Feature/docker improvements march 2022 (#235) + * minor changes + * replanning for all our examples + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dance_bot visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dance_bot (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * more refinement in sm_dance_bot + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dance_bot + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dance_bot + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dance_bot tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nav2z renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dance_bot + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + * some reordering fixes + * minor + * docker files for different revisions, warnings removval and more testing on navigation + * fixing docker for foxy and galactic + * docker build files for all versions + * barrel demo + * barrel search build fix and warehouse3 + * fixing startup problems in warehouse 3 + * fixing format and minor + * minor + * progress in barrel husky + * minor + * barrel demo + * minor + * barrel search updates + * making models local + * red picuup + * multiple controllable leds plugin + * progress in husky demo + * progressing in husky demo + * improving navigation behaviors + * more merge + * docker improvements + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + * Use correct upstream .repos files for source builds (#243) + * Correct mergify branch names (#246) + * Correct name of source-build job and bump version of action (#242) (#247) + Co-authored-by: Denis Štogl + Co-authored-by: Pablo Iñigo Blasco + * Update galactic source build job name (#250) + * Galactic source build: update .repos file, bump action version and use correct version of upstream packages (backport #241) (#248) + Co-authored-by: Denis Štogl + * fixing rolling build (#239) + * fixing rolling build + * trying to fix dependencies + * missing repo + * fixing to focal by the moment + * more fixing rolling build + * minor + * cache matrix rolling and source build package + * minor + * minor + * missing repo + * missing deps + * fixing building issue + * typo + * fixing broken build + * build fix + * restoring workflow files (#252) + * restoring files (#253) + * Fix checkout branches for scheduled builds (#254) + * correct checkout branch on scheduled build + * Update foxy-source-build.yml + * Feature/fixing husky build rolling (#257) + * restoring files + * making husky project build on rolling + * Feature/fixing husky build rolling (#258) + * restoring files + * making husky project build on rolling + * husky progress + * Update README.md (#262) + * Feature/fixing ur demos (#261) + * restoring files + * fixes + * Feature/fixing type string walker (#263) + * restoring files + * fixing type string walker threesome demo + * Update README.md (#266) + * Update README.md (#267) + * Update README.md (#268) + * Significant update in Getting Started Instructions (#269) + * Significant update in Getting Started Instructions + * Remove trailing spaces. + Co-authored-by: Denis Štogl + * fixing ur demo (#273) + * fix: initialise conditionFlag as false (#274) + * precommit fix (#280) + I merge this in red for focal-rolling because it was already broken anyway and it is only a minor update of the precommit + * Ignore packages which should not be released. + * Added changelogs. + * 0.4.0 + * Revert "Ignore packages which should not be released." + This reverts commit ee2cc86db3c0a24f9eb0a9e33217de3f7a691a1c. + * Fix urls to index.ros.org (#284) + * Fix foxy source build config to use repos file from foxy branch. (#285) + * adding spawn entity delays + * more on backport + * more on backport + * disappeared ur_msgs denis repo + * fixing sm_dance_bot examples + * working on fix of image messages for husky_barrel demo + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + Co-authored-by: mergify[bot] <37929162+mergify[bot]@users.noreply.github.com> + Co-authored-by: brettpac +* Revert "Ignore packages which should not be released." + This reverts commit dec14a936a877b2ef722a6a32f1bf3df09312542. +* Ignore packages which should not be released. +* Feature/master rolling to galactic backport (#236) + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dance_bot visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dance_bot (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * more refinement in sm_dance_bot + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dance_bot + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dance_bot + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dance_bot tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nav2z renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dance_bot + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * replanning for all our examples + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dance_bot visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dance_bot (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * more refinement in sm_dance_bot + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dance_bot + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dance_bot + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dance_bot tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nav2z renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dance_bot + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * Update cb_navigate_global_position.hpp + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + * Merging code from backport foxy and updates about autoware (#208) + * minor changes + * replanning for all our examples + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dance_bot visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dance_bot (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * more refinement in sm_dance_bot + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dance_bot + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dance_bot + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dance_bot tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nav2z renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dance_bot + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + * Add mergify rules file. + * Try fixing CI for rolling. (#209) + Merging to get backport working. + * some reordering fixes + * Remove example things from Foxy CI setup. (#214) + * Add Autoware Auto Msgs into not-released dependencies. (#220) + * Fix rolling builds (#222) + * do not merge yet - Feature/odom tracker improvements and retry motion (#223) + * odom tracker improvements + * adding forward behavior retry funcionality + * minor + * docker files for different revisions, warnings removval and more testing on navigation + * fixing docker for foxy and galactic + * removing warnings (#213) + * minor changes + * replanning for all our examples + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dance_bot visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dance_bot (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * more refinement in sm_dance_bot + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dance_bot + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dance_bot + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dance_bot tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nav2z renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dance_bot + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + * some reordering fixes + * minor + * docker files for different revisions, warnings removval and more testing on navigation + * fixing docker for foxy and galactic + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + * docker build files for all versions + * dockerfiles (#225) + * Fix code generators (#221) + * Fix other build issues. + * Update SM template and make example code clearly visible. + * Remove use of node in the sm performance template. + * Updated templated to use Blackboard storage. + * Update template to resolve the global data correctly. + * Update sm_name.hpp + Co-authored-by: Pablo Iñigo Blasco + * Feature/retry behavior warehouse 1 (#226) + * minor changes + * replanning for all our examples + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dance_bot visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dance_bot (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * more refinement in sm_dance_bot + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dance_bot + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dance_bot + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dance_bot tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nav2z renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dance_bot + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + * some reordering fixes + * minor + * docker files for different revisions, warnings removval and more testing on navigation + * fixing docker for foxy and galactic + * Update file for fake hardware simulation and add file for gazebo simulation. + * docker build files for all versions + * retry behavior warehouse 1 + * missing file + * minor format fix + * other minor changes + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + * barrel demo + * barrel search build fix and warehouse3 + * fixing startup problems in warehouse 3 + * fix broken source build (#227) + * fixing format and minor + * minor + * progress in barrel husky + * minor + * Only rolling version should be pre-released on on master. (#230) + * barrel demo + * minor + * barrel search updates + * making models local + * red picuup + * Correct Focal-Rolling builds by fixing the version of rosdep yaml (#234) + * multiple controllable leds plugin + * progress in husky demo + * Update file for fake hardware simulation and add file for gazebo simulation. (#224) + * Update file for fake hardware simulation and add file for gazebo simulation. + * Add ignition file and update repos files. + * progressing in husky demo + * improving navigation behaviors + * Feature/improvements warehouse3 (#228) + * minor changes + * replanning for all our examples + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dance_bot visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dance_bot (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * more refinement in sm_dance_bot + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dance_bot + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dance_bot + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dance_bot tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nav2z renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dance_bot + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + * some reordering fixes + * minor + * docker files for different revisions, warnings removval and more testing on navigation + * fixing docker for foxy and galactic + * docker build files for all versions + * barrel demo + * barrel search build fix and warehouse3 + * fixing startup problems in warehouse 3 + * fixing format and minor + * minor + * progress in barrel husky + * minor + * barrel demo + * progress + * fixing broken build + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + * more merge + * docker improvements + * master rolling to galactic backport + * fixing build + * testing dance bot demos + * updating galactic repos + * runtime dependency + * restoring ur dependency + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco +* Backport/initial to galactic (#61) + * reformatting the whole project + * Remove test phase from CMake and remove dependencies from package.xml. + * Compile with navigation and slam_toolbox. + * Enable all packages to compile. + * Resolve missing dependency in smacc_msgs and reorganize them for better overview. + * getLogger functionality and refactoring + * broken sm_respira + * sm_respira code + * Update README.md + ## Additions + - build-status table + - detailed install instructions (adjusted from [here](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver#readme)) + ## Changes + - default build type as `Release` (it is faster than `Debug` and executables are smaller) + - updated examples section + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Update tracing/ManualTracing.md + * reactivating smacc2 nav clients for rolling via submodules + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * edited tracing.md to reflect new tracing event names + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * Correct build-overview table. + * Update and unify CI configurations. + * Use tf_geometry_msgs.h in galactic. + * Use galactic branches in .repos-file. + Co-authored-by: pabloinigoblasco + Co-authored-by: reelrbtx + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: brettpac +* Contributors: Denis Štogl, Pablo Iñigo Blasco, pabloinigoblasco diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/CMakeLists.txt b/smacc2_sm_reference_library/sm_dancebot_ue/CMakeLists.txt index 5ae4e6577..25b52357d 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/CMakeLists.txt +++ b/smacc2_sm_reference_library/sm_dancebot_ue/CMakeLists.txt @@ -18,6 +18,13 @@ find_package(Boost COMPONENTS thread REQUIRED) find_package(smacc2 REQUIRED) find_package(std_msgs REQUIRED) find_package(nav2z_client REQUIRED) + +find_package(ros_publisher_client REQUIRED) +find_package(multirole_sensor_client REQUIRED) + +find_package(sr_all_events_go REQUIRED) +find_package(sr_event_countdown REQUIRED) +find_package(sr_conditional REQUIRED) find_package(ros_timer_client REQUIRED) find_package(sensor_msgs REQUIRED) find_package(visualization_msgs REQUIRED) @@ -27,50 +34,54 @@ set(dependencies std_msgs sensor_msgs nav2z_client + ros_publisher_client + multirole_sensor_client + sr_all_events_go + sr_event_countdown + sr_conditional ros_timer_client visualization_msgs ) -# rosidl_generate_interfaces(${PROJECT_NAME} -# "servers/led_action_server/action/LEDControl.action" -# DEPENDENCIES builtin_interfaces std_msgs action_msgs sensor_msgs -# ) +rosidl_generate_interfaces(${PROJECT_NAME} + "servers/led_action_server/action/LEDControl.action" + DEPENDENCIES builtin_interfaces std_msgs action_msgs sensor_msgs +) include_directories(include ${Boost_INCLUDE_DIRS} ${CMAKE_BINARY_DIR}/rosidl_generator_cpp) add_executable(${PROJECT_NAME}_node - src/${PROJECT_NAME}/sm_dancebot_ue_node.cpp - # src/${PROJECT_NAME}/clients/cl_led/cl_led.cpp + src/${PROJECT_NAME}/sm_dancebot_ue.cpp + src/${PROJECT_NAME}/clients/cl_led/cl_led.cpp ) -# add_executable(temperature_sensor_node_${PROJECT_NAME} servers/temperature_sensor_node/src/temperature_sensor_node.cpp) -# add_executable(led_action_server_node_${PROJECT_NAME} servers/led_action_server/src/led_action_server_node.cpp) +add_executable(temperature_sensor_node_${PROJECT_NAME} servers/temperature_sensor_node/src/temperature_sensor_node.cpp) +add_executable(led_action_server_node_${PROJECT_NAME} servers/led_action_server/src/led_action_server_node.cpp) ament_target_dependencies(${PROJECT_NAME}_node ${dependencies}) -#target_link_libraries(${PROJECT_NAME}_node ${Boost_LIBS} ${PROJECT_NAME}__rosidl_typesupport_cpp) +target_link_libraries(${PROJECT_NAME}_node ${Boost_LIBS} ${PROJECT_NAME}__rosidl_typesupport_cpp) -# ament_target_dependencies(led_action_server_node_${PROJECT_NAME} ${dependencies}) -# set_target_properties(led_action_server_node_${PROJECT_NAME} PROPERTIES OUTPUT_NAME "led_action_server_node") -# target_link_libraries(led_action_server_node_${PROJECT_NAME} ${Boost_LIBS} ${PROJECT_NAME}__rosidl_typesupport_cpp) +ament_target_dependencies(led_action_server_node_${PROJECT_NAME} ${dependencies}) +set_target_properties(led_action_server_node_${PROJECT_NAME} PROPERTIES OUTPUT_NAME "led_action_server_node") +target_link_libraries(led_action_server_node_${PROJECT_NAME} ${Boost_LIBS} ${PROJECT_NAME}__rosidl_typesupport_cpp) -# ament_target_dependencies(temperature_sensor_node_${PROJECT_NAME} ${dependencies}) -# set_target_properties(temperature_sensor_node_${PROJECT_NAME} PROPERTIES OUTPUT_NAME "temperature_sensor_node") +ament_target_dependencies(temperature_sensor_node_${PROJECT_NAME} ${dependencies}) +set_target_properties(temperature_sensor_node_${PROJECT_NAME} PROPERTIES OUTPUT_NAME "temperature_sensor_node") ament_export_include_directories(include) ament_export_libraries(${PROJECT_NAME}) ament_export_dependencies(${dependencies}) -# ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/env-hooks/sm_dancebot_ue.dsv.in") if(BUILD_TESTING) endif() install(TARGETS ${PROJECT_NAME}_node - # temperature_sensor_node_${PROJECT_NAME} - # led_action_server_node_${PROJECT_NAME} + temperature_sensor_node_${PROJECT_NAME} + led_action_server_node_${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -83,17 +94,15 @@ install(TARGETS install(FILES - # servers/service_node_3/service_node_3.py + servers/service_node_3/service_node_3.py DESTINATION lib/${PROJECT_NAME} PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READ GROUP_EXECUTE GROUP_READ) install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) -#install(DIRECTORY worlds DESTINATION share/${PROJECT_NAME}) install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME}) install(DIRECTORY params DESTINATION share/${PROJECT_NAME}) install(DIRECTORY urdf DESTINATION share/${PROJECT_NAME}) -#install(DIRECTORY models DESTINATION share/${PROJECT_NAME}) ament_package() diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/README.md b/smacc2_sm_reference_library/sm_dancebot_ue/README.md new file mode 100644 index 000000000..8fa9efb95 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/README.md @@ -0,0 +1,55 @@ +

State Machine Diagram

+ + ![sm_dance_bot](docs/SmDanceBot_2023-5-26_151817.svg) + +

Description

A full-featured state machine example, that highlights the capabilities of SMACC2 & the ROS2 Navigation Stack via the MoveBaseZ Client. +.

+Doxygen Namespace & Class Reference + +

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 ~/colcon_ws/install/setup.bash +``` + +And then run the launch file... + +``` +ros2 launch sm_dance_bot sm_dance_bot_launch.py +``` +

Headless launch

+ +Alternatively, you can also launch the gazebo simulator in headless mode: +``` +ros2 launch sm_dance_bot sm_dance_bot_launch.py headless:=True +``` +

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_dancebot_ue/include/sm_dancebot_ue/clients/cl_led/cl_led.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_led/cl_led.hpp new file mode 100644 index 000000000..78d0632e9 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_led/cl_led.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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#pragma once + +#include +#include + +namespace sm_dancebot_ue +{ +namespace cl_led +{ +class ClLED +: public smacc2::client_bases::SmaccActionClientBase +{ +public: + + ClLED(std::string actionServerName); + virtual std::string getName() const override; + virtual ~ClLED(); +}; + +std::ostream & operator<<( + std::ostream & out, const sm_dancebot_ue::action::LEDControl::Goal & msg); + +} // namespace cl_led +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_led/client_behaviors/cb_led_off.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_led/client_behaviors/cb_led_off.hpp new file mode 100644 index 000000000..8ea3b3dc8 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_led/client_behaviors/cb_led_off.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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#pragma once + +#include +#include + +namespace sm_dancebot_ue +{ +namespace cl_led +{ +class CbLEDOff : public smacc2::SmaccClientBehavior +{ +public: + cl_led::ClLED * ledActionClient_; + + void onEntry() override + { + this->requiresClient(ledActionClient_); + + cl_led::ClLED::Goal goal; + goal.command = cl_led::ClLED::Goal::CMD_OFF; + ledActionClient_->sendGoal(goal); + } + + void onExit() override + { + //RCLCPP_INFO(getLogger(),"Entering ToolClientBehavior"); + } +}; +} // namespace cl_led +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_led/client_behaviors/cb_led_on.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_led/client_behaviors/cb_led_on.hpp new file mode 100644 index 000000000..6a3d57bb2 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_led/client_behaviors/cb_led_on.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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#pragma once + +#include +#include + +namespace sm_dancebot_ue +{ +namespace cl_led +{ +class CbLEDOn : public smacc2::SmaccClientBehavior +{ +public: + cl_led::ClLED * ledActionClient_; + + void onEntry() override + { + this->requiresClient(ledActionClient_); + + cl_led::ClLED::Goal goal; + goal.command = cl_led::ClLED::Goal::CMD_ON; + ledActionClient_->sendGoal(goal); + } + + void onExit() override + { + //RCLCPP_INFO(getLogger(),"Entering ToolClientBehavior"); + } +}; +} // namespace cl_led +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_lidar/cl_lidar.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_lidar/cl_lidar.hpp new file mode 100644 index 000000000..77d1f4cb7 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_lidar/cl_lidar.hpp @@ -0,0 +1,38 @@ +// 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 + +namespace sm_dancebot_ue +{ +namespace cl_lidar +{ +class ClLidarSensor : public cl_multirole_sensor::ClMultiroleSensor +{ +public: + ClLidarSensor() {} +}; +} // namespace cl_lidar +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_lidar/client_behaviors/cb_lidar_sensor.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_lidar/client_behaviors/cb_lidar_sensor.hpp new file mode 100644 index 000000000..fb60c30aa --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_lidar/client_behaviors/cb_lidar_sensor.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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#pragma once + +#include +#include +#include + +namespace sm_dancebot_ue +{ +namespace cl_lidar +{ +struct CbLidarSensor : cl_multirole_sensor::CbDefaultMultiRoleSensorBehavior +{ +public: + CbLidarSensor() {} + + void onEntry() override + { + RCLCPP_INFO(getLogger(), "[CbLidarSensor] onEntry"); + cl_multirole_sensor::CbDefaultMultiRoleSensorBehavior::onEntry(); + } + + virtual void onMessageCallback(const sensor_msgs::msg::LaserScan & /*msg*/) override {} +}; +} // namespace cl_lidar +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_service3/cl_service3.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_service3/cl_service3.hpp new file mode 100644 index 000000000..b54098119 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_service3/cl_service3.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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#pragma once + +#include +#include +namespace sm_dancebot_ue +{ +namespace cl_service3 +{ +class ClService3 : public smacc2::client_bases::SmaccServiceClient +{ +public: + ClService3() {} +}; +} // namespace cl_service3 +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_service3/client_behaviors/cb_service3.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_service3/client_behaviors/cb_service3.hpp new file mode 100644 index 000000000..268ee9e24 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_service3/client_behaviors/cb_service3.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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#include +#include + +namespace sm_dancebot_ue +{ +namespace cl_service3 +{ +enum class Service3Command +{ + SERVICE3_ON, + SERVICE3_OFF +}; + +class CbService3 : public smacc2::SmaccClientBehavior +{ +private: + ClService3 * serviceClient_; + Service3Command value_; + +public: + CbService3(Service3Command value) { value_ = value; } + + void onEntry() override + { + this->requiresClient(serviceClient_); + + auto req = std::make_shared(); + if (value_ == Service3Command::SERVICE3_ON) + req->data = true; + else + req->data = false; + + serviceClient_->call(req); + } +}; +} // namespace cl_service3 +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_string_publisher/cl_string_publisher.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_string_publisher/cl_string_publisher.hpp new file mode 100644 index 000000000..7d757c7a0 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_string_publisher/cl_string_publisher.hpp @@ -0,0 +1,40 @@ +// 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 + +namespace sm_dancebot_ue +{ +namespace cl_string_publisher +{ +class ClStringPublisher : public smacc2::client_bases::SmaccPublisherClient +{ +public: + ClStringPublisher(std::string topicName) : topicName_(topicName) {} + + void onInitialize() override { this->configure(topicName_); } + + std::string topicName_; +}; +} // namespace cl_string_publisher +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_string_publisher/client_behaviors/cb_string_publisher.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_string_publisher/client_behaviors/cb_string_publisher.hpp new file mode 100644 index 000000000..5627d5e76 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_string_publisher/client_behaviors/cb_string_publisher.hpp @@ -0,0 +1,55 @@ +// 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 + +namespace sm_dancebot_ue +{ +namespace cl_string_publisher +{ +class CbStringPublisher : public smacc2::SmaccClientBehavior +{ +public: + ClStringPublisher * publisherClient_; + std::string msg_; + + CbStringPublisher(std::string msg) { msg_ = msg; } + + virtual void runtimeConfigure() override + { + RCLCPP_INFO_STREAM( + getLogger(), "Creating CbStringPublisher behavior with stored message: " << msg_); + } + + virtual void onEntry() { this->requiresClient(publisherClient_); } + + void onExit() override + { + std_msgs::msg::String rosmsg; + rosmsg.data = msg_; + publisherClient_->publish(rosmsg); + } +}; +} // namespace cl_string_publisher +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_temperature_sensor/cl_temperature_sensor.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_temperature_sensor/cl_temperature_sensor.hpp new file mode 100644 index 000000000..2dbfba15d --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_temperature_sensor/cl_temperature_sensor.hpp @@ -0,0 +1,38 @@ +// 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 + +namespace sm_dancebot_ue +{ +namespace cl_temperature_sensor +{ +class ClTemperatureSensor +: public cl_multirole_sensor::ClMultiroleSensor +{ +public: + ClTemperatureSensor() {} +}; +} // namespace cl_temperature_sensor +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_temperature_sensor/client_behaviors/cb_custom_condition_temperature_sensor.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_temperature_sensor/client_behaviors/cb_custom_condition_temperature_sensor.hpp new file mode 100644 index 000000000..47c6d9939 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_temperature_sensor/client_behaviors/cb_custom_condition_temperature_sensor.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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#pragma once +#include +#include +#include + +namespace sm_dancebot_ue +{ +namespace cl_temperature_sensor +{ +struct EvCustomTemperatureAlert : sc::event +{ +}; + +//-------------------------------------------------------------------------------------- +class CbConditionTemperatureSensor +: public cl_multirole_sensor::CbDefaultMultiRoleSensorBehavior +{ +public: + CbConditionTemperatureSensor() {} + void onEntry() override + { + RCLCPP_INFO(getLogger(), "[CbConditionTemperatureSensor] onEntry"); + cl_multirole_sensor::CbDefaultMultiRoleSensorBehavior::onEntry(); + } + + void onMessageCallback(const sensor_msgs::msg::Temperature & msg) override + { + if (msg.temperature > 40) + { + auto ev = new EvCustomTemperatureAlert(); + this->postEvent(ev); + } + } +}; +} // namespace cl_temperature_sensor +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/modestates/ms_dance_bot_recovery_mode.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/modestates/ms_dance_bot_recovery_mode.hpp new file mode 100644 index 000000000..18ac2c5f3 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/modestates/ms_dance_bot_recovery_mode.hpp @@ -0,0 +1,38 @@ +// 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 + * + ******************************************************************************************************************/ + +#include +namespace sm_dancebot_ue +{ +// STATE DECLARATION +class MsDanceBotRecoveryMode : public smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition> + + >reactions; + // typedef Transition reactions; +}; +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/modestates/ms_dance_bot_run_mode.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/modestates/ms_dance_bot_run_mode.hpp new file mode 100644 index 000000000..a8a9b44ec --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/modestates/ms_dance_bot_run_mode.hpp @@ -0,0 +1,37 @@ +// 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 + * + ******************************************************************************************************************/ + +#include +namespace sm_dancebot_ue +{ +// STATE DECLARATION +class MsDanceBotRunMode : public smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition + + >reactions; +}; +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_led.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_led.hpp new file mode 100644 index 000000000..c85084d3e --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_led.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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#pragma once +#include +#include + +namespace sm_dancebot_ue +{ +class OrLED : public smacc2::Orthogonal +{ +public: + void onInitialize() override + { + auto actionclient = this->createClient("led_action_server"); + } +}; +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_navigation.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_navigation.hpp new file mode 100644 index 000000000..f664c132a --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_navigation.hpp @@ -0,0 +1,88 @@ +// 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 +#include + +namespace sm_dancebot_ue +{ +using namespace cl_nav2z; +using namespace std::chrono_literals; + +class OrNavigation : public smacc2::Orthogonal +{ +public: + void onInitialize() override + { + auto nav2zClient = this->createClient(); + + // create pose component + nav2zClient->createComponent(); + + // create planner switcher + nav2zClient->createComponent(); + + // create goal checker switcher + nav2zClient->createComponent(); + + // create odom tracker + nav2zClient->createComponent(); + + // create odom tracker + nav2zClient->createComponent(); + + // create waypoints navigator component + auto waypointsNavigator = nav2zClient->createComponent(); + loadWaypointsFromYaml(waypointsNavigator); + + // change this to skip some points of the yaml file, default = 0 + waypointsNavigator->currentWaypoint_ = 0; + } + + void loadWaypointsFromYaml(CpWaypointNavigator * waypointsNavigator) + { + // if it is the first time and the waypoints navigator is not configured + std::string planfilepath; + getNode()->declare_parameter("waypoints_plan", planfilepath); + if (getNode()->get_parameter("waypoints_plan", planfilepath)) + { + std::string package_share_directory = + ament_index_cpp::get_package_share_directory("sm_dancebot_ue"); + boost::replace_all(planfilepath, "$(pkg_share)", package_share_directory); + waypointsNavigator->loadWayPointsFromFile(planfilepath); + RCLCPP_INFO(getLogger(), "waypoints plan: %s", planfilepath.c_str()); + } + else + { + RCLCPP_ERROR(getLogger(), "waypoints plan file not found: NONE"); + } + } +}; +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_obstacle_perception.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_obstacle_perception.hpp new file mode 100644 index 000000000..57acbc326 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_obstacle_perception.hpp @@ -0,0 +1,40 @@ +// 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 + +namespace sm_dancebot_ue +{ + using namespace std::chrono_literals; +class OrObstaclePerception : public smacc2::Orthogonal +{ +public: + void onInitialize() override + { + auto lidarClient = this->createClient(); + + lidarClient->topicName = "/scan"; + lidarClient->timeout_ = rclcpp::Duration(10s); + } +}; +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_service3.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_service3.hpp new file mode 100644 index 000000000..c9ef9f1d9 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_service3.hpp @@ -0,0 +1,37 @@ +// 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 + +namespace sm_dancebot_ue +{ +class OrService3 : public smacc2::Orthogonal +{ +public: + void onInitialize() override + { + auto serviceClient = this->createClient(); + serviceClient->serviceName_ = "/service_node3"; + } +}; +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_string_publisher.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_string_publisher.hpp new file mode 100644 index 000000000..433cac9fb --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_string_publisher.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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#pragma once + +#include +#include +namespace sm_dancebot_ue +{ +class OrStringPublisher : public smacc2::Orthogonal +{ +public: + void onInitialize() override { this->createClient("/string_publisher_out"); } +}; +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_temperature_sensor.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_temperature_sensor.hpp new file mode 100644 index 000000000..d8e35742b --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_temperature_sensor.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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#pragma once + +#include +#include +#include +#include + +using namespace std::chrono_literals; + +namespace sm_dancebot_ue +{ +class OrTemperatureSensor : public smacc2::Orthogonal +{ +public: + void onInitialize() override + { + auto clTemperatureSensor = this->createClient(); + + clTemperatureSensor->topicName = "/temperature"; + clTemperatureSensor->timeout_ = rclcpp::Duration(10s); + } +}; +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_timer.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_timer.hpp new file mode 100644 index 000000000..7ae1ad8d7 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_timer.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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#pragma once +#include +#include + +namespace sm_dancebot_ue +{ +class OrTimer : public smacc2::Orthogonal +{ +public: + void onInitialize() override + { + auto actionclient = this->createClient( + rclcpp::Duration(std::chrono::milliseconds(500))); + } +}; +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_updatable_publisher.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_updatable_publisher.hpp new file mode 100644 index 000000000..960bdf725 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_updatable_publisher.hpp @@ -0,0 +1,37 @@ +// 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 + +namespace sm_dancebot_ue +{ +class OrUpdatablePublisher : public smacc2::Orthogonal +{ +public: + void onInitialize() override + { + auto publisherClient_ = this->createClient(); + publisherClient_->configure("/updatable_string_publisher_out"); + } +}; +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/sm_dancebot_ue.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/sm_dancebot_ue.hpp new file mode 100644 index 000000000..15024f8fb --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/sm_dancebot_ue.hpp @@ -0,0 +1,198 @@ +// 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 + * + ******************************************************************************************************************/ + +#include + +#include + +// CLIENT BEHAVIORS +#include + +#include + +#include +#include + +using namespace cl_nav2z; + +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include + +#include + +using namespace sm_dancebot_ue::cl_lidar; +using namespace sm_dancebot_ue::cl_service3; +using namespace sm_dancebot_ue::cl_string_publisher; +using namespace sm_dancebot_ue::cl_temperature_sensor; +using namespace sm_dancebot_ue::cl_led; + +//STATE REACTORS +#include +#include +#include + +using namespace smacc2::state_reactors; + +// ORTHOGONALS +#include +#include +#include +#include +#include +#include +#include +#include + +namespace sm_dancebot_ue +{ +//STATE FORWARD DECLARATIONS +class StAcquireSensors; +class StEventCountDown; + +class StNavigateForward1; +class StNavigateToWaypoint1; +class StNavigateToWaypointsX; + +class StRotateDegrees1; +class StRotateDegrees2; +class StRotateDegrees3; +class StRotateDegrees4; +class StRotateDegrees5; +class StRotateDegrees6; + +class StNavigateReverse1; +class StNavigateReverse2; +class StNavigateReverse3; +class StNavigateReverse4; + +//SUPERSTATE FORWARD DECLARATIONS +//MODE STATES FORWARD DECLARATIONS +class MsDanceBotRunMode; +class MsDanceBotRecoveryMode; + +namespace SS1 +{ +class SsRadialPattern1; +} + +namespace SS2 +{ +class SsRadialPattern2; +} + +namespace SS3 +{ +class SsRadialPattern3; +} + +namespace SS4 +{ +class SsFPattern1; +} + +namespace SS5 +{ +class SsSPattern1; +} + +// custom smd_dance_bot event +struct EvGlobalError : sc::event +{ +}; + +} // namespace sm_dancebot_ue + +using namespace sm_dancebot_ue; +using namespace cl_ros_timer; +using namespace smacc2; + +namespace sm_dancebot_ue +{ +/// \brief Advanced example of state machine with smacc that shows multiple techniques +/// for the development of state machines +struct SmDanceBot : public smacc2::SmaccStateMachineBase +{ + int counter_1; + bool rt_ready_flag; + + typedef mpl::bool_ shallow_history; + typedef mpl::bool_ deep_history; + typedef mpl::bool_ inherited_deep_history; + + using SmaccStateMachineBase::SmaccStateMachineBase; + + void onInitialize() override + { + this->setGlobalSMData("counter_1", counter_1); + this->setGlobalSMData("rt_ready_flag", rt_ready_flag); + + this->createOrthogonal(); + this->createOrthogonal(); + this->createOrthogonal(); + this->createOrthogonal(); + this->createOrthogonal(); + this->createOrthogonal(); + this->createOrthogonal(); + this->createOrthogonal(); + } +}; + +} // namespace sm_dancebot_ue + +//MODE STATES +#include + +#include + +//SUPERSTATES +#include +#include +#include +#include +#include + +//STATES +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_forward_1.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_forward_1.hpp new file mode 100644 index 000000000..a3e076242 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_forward_1.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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +namespace sm_dancebot_ue +{ +namespace f_pattern_states +{ +// STATE DECLARATION +template +struct StiFPatternForward1 : public smacc2::SmaccState, SS> +{ + typedef SmaccState, SS> TSti; + using TSti::context_type; + using TSti::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiFPatternReturn1>, + Transition, StiFPatternReturn1, ABORT> + + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + TSti::template configure_orthogonal(SS::ray_lenght_meters()); + TSti::template configure_orthogonal(); + TSti::template configure_orthogonal(); + } + + void runtimeConfigure() + { + // auto &superstate = TSti::template context(); + // RCLCPP_INFO(this->getLogger(),"[SsrFpattern] Fpattern rotate: SS current iteration: %d/%d", + // superstate.iteration_count, SS::total_iterations()); + } +}; +} // namespace f_pattern_states +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_forward_2.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_forward_2.hpp new file mode 100644 index 000000000..2693c7600 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_forward_2.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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +namespace sm_dancebot_ue +{ +namespace f_pattern_states +{ +// STATE DECLARATION +template +struct StiFPatternForward2 : smacc2::SmaccState, SS> +{ + typedef SmaccState, SS> TSti; + using TSti::context_type; + using TSti::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiFPatternStartLoop>, + Transition, StiFPatternStartLoop> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + TSti::template configure_orthogonal(SS::pitch_lenght_meters()); + TSti::template configure_orthogonal(); + TSti::template configure_orthogonal(); + } + + void runtimeConfigure() {} +}; +} // namespace f_pattern_states +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_loop_start.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_loop_start.hpp new file mode 100644 index 000000000..018d3f171 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_loop_start.hpp @@ -0,0 +1,55 @@ +// 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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_ue +{ +namespace f_pattern_states +{ +// STATE DECLARATION +template +struct StiFPatternStartLoop : smacc2::SmaccState, SS> +{ + typedef SmaccState, SS> TSti; + using TSti::context_type; + using TSti::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition>, StiFPatternRotate1, CONTINUELOOP> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + bool loopCondition() + { + auto & superstate = TSti::template context(); + return superstate.iteration_count++ < superstate.total_iterations(); + } + + void onEntry() + { + TSti::checkWhileLoopConditionAndThrowEvent(&StiFPatternStartLoop::loopCondition); + } +}; +} // namespace f_pattern_states +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_return_1.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_return_1.hpp new file mode 100644 index 000000000..bd54a79fa --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_return_1.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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +namespace sm_dancebot_ue +{ +namespace f_pattern_states +{ +// STATE DECLARATION +template +struct StiFPatternReturn1 : smacc2::SmaccState, SS> +{ + typedef SmaccState, SS> TSti; + using TSti::context_type; + using TSti::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiFPatternRotate2> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + TSti::template configure_orthogonal(); + TSti::template configure_orthogonal(); + TSti::template configure_orthogonal(); + } + + void runtimeConfigure() {} +}; +} // namespace f_pattern_states +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_rotate_1.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_rotate_1.hpp new file mode 100644 index 000000000..c9a6c1945 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_rotate_1.hpp @@ -0,0 +1,61 @@ +// 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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_ue +{ +namespace f_pattern_states +{ +// STATE DECLARATION +template +struct StiFPatternRotate1 : smacc2::SmaccState, SS> +{ + typedef SmaccState, SS> TSti; + using TSti::context_type; + using TSti::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiFPatternForward1>, + Transition, StiFPatternRotate1> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + float angle = 0; + double offset = -1.5; // for a better behaving + + if (SS::direction() == TDirection::LEFT) + angle = 90 + offset; + else + angle = -90 - offset; + + //TSti::template configure_orthogonal(angle); + TSti::template configure_orthogonal( + angle); // absolute aligned to the y-axis + TSti::template configure_orthogonal(); + } + + void runtimeConfigure() {} +}; +} // namespace f_pattern_states +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_rotate_2.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_rotate_2.hpp new file mode 100644 index 000000000..84a25912a --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_rotate_2.hpp @@ -0,0 +1,63 @@ +// 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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_ue +{ +namespace f_pattern_states +{ +// STATE DECLARATION +template +struct StiFPatternRotate2 : smacc2::SmaccState, SS> +{ + typedef SmaccState, SS> TSti; + using TSti::context_type; + using TSti::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiFPatternForward2>, + Transition, StiFPatternRotate2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + double offset = 1.5; // for a better behaving + // float angle = 0; + + // if (SS::direction() == TDirection::LEFT) + // angle = -90 - offset; + // else + // angle = 90 + offset; + + //TSti::template configure_orthogonal(angle); + TSti::template configure_orthogonal( + 0 + offset); // absolute horizontal + TSti::template configure_orthogonal(); + + TSti::template configure_orthogonal(); + } + + void runtimeConfigure() {} +}; +} // namespace f_pattern_states +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/radial_motion_states/sti_radial_end_point.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/radial_motion_states/sti_radial_end_point.hpp new file mode 100644 index 000000000..db38080c1 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/radial_motion_states/sti_radial_end_point.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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +namespace sm_dancebot_ue +{ +namespace radial_motion_states +{ +// STATE DECLARATION +struct StiRadialEndPoint : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiRadialReturn, SUCCESS>, + Transition, StiRadialReturn, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + //RCLCPP_INFO(getLogger(),"ssr radial end point, distance in meters: %lf", SS::ray_length_meters()); + configure_orthogonal(SS::ray_length_meters()); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} +}; +} // namespace radial_motion_states +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/radial_motion_states/sti_radial_loop_start.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/radial_motion_states/sti_radial_loop_start.hpp new file mode 100644 index 000000000..dd378e441 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/radial_motion_states/sti_radial_loop_start.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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +namespace sm_dancebot_ue +{ +namespace radial_motion_states +{ +// STATE DECLARATION +struct StiRadialLoopStart : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiRadialRotate, 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(&StiRadialLoopStart::loopWhileCondition); + } +}; +} // namespace radial_motion_states +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/radial_motion_states/sti_radial_return.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/radial_motion_states/sti_radial_return.hpp new file mode 100644 index 000000000..8050092f9 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/radial_motion_states/sti_radial_return.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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +namespace sm_dancebot_ue +{ +namespace radial_motion_states +{ +// STATE DECLARATION +struct StiRadialReturn : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiRadialLoopStart, SUCCESS>, + Transition, StiRadialEndPoint, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(); + configure_orthogonal(); + configure_orthogonal(); + } + + void onExit() + { + ClNav2Z * moveBase; + this->requiresClient(moveBase); + + auto odomTracker = moveBase->getComponent(); + odomTracker->clearPath(); + } +}; +} // namespace radial_motion_states +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/radial_motion_states/sti_radial_rotate.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/radial_motion_states/sti_radial_rotate.hpp new file mode 100644 index 000000000..522de035e --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/radial_motion_states/sti_radial_rotate.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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +namespace sm_dancebot_ue +{ +namespace radial_motion_states +{ +// STATE DECLARATION +struct StiRadialRotate : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiRadialEndPoint, SUCCESS>, + Transition, StiRadialRotate, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() + { + auto cbAbsRotate = this->getClientBehavior(); + + cbAbsRotate->spinningPlanner = SpinningPlanner::PureSpinning; + + auto & superstate = this->context(); + cbAbsRotate->absoluteGoalAngleDegree = + superstate.iteration_count * SS::ray_angle_increment_degree(); + } +}; +} // namespace radial_motion_states +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_forward_1.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_forward_1.hpp new file mode 100644 index 000000000..d10607157 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_forward_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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +namespace sm_dancebot_ue +{ +namespace s_pattern_states +{ +// STATE DECLARATION +struct StiSPatternForward1 : public smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSPatternRotate2>, + Transition, StiSPatternRotate1> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(); + configure_orthogonal(SS::pitch1_lenght_meters()); + configure_orthogonal(); + } + + void runtimeConfigure() {} +}; +} // namespace s_pattern_states +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_forward_2.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_forward_2.hpp new file mode 100644 index 000000000..a60d30f30 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_forward_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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +namespace sm_dancebot_ue +{ +namespace s_pattern_states +{ +// STATE DECLARATION +struct StiSPatternForward2 : public smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSPatternRotate3>, + Transition, StiSPatternRotate2> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(SS::pitch2_lenght_meters()); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() {} +}; +} // namespace s_pattern_states +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_forward_3.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_forward_3.hpp new file mode 100644 index 000000000..f677f2ac5 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_forward_3.hpp @@ -0,0 +1,47 @@ +// 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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_ue +{ +namespace s_pattern_states +{ +// STATE DECLARATION +struct StiSPatternForward3 : public smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSPatternRotate4>, + Transition, StiSPatternRotate3> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(SS::pitch1_lenght_meters()); + configure_orthogonal(); + configure_orthogonal(); + } +}; +} // namespace s_pattern_states +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_forward_4.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_forward_4.hpp new file mode 100644 index 000000000..2e33e06ae --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_forward_4.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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +namespace sm_dancebot_ue +{ +namespace s_pattern_states +{ +// STATE DECLARATION +struct StiSPatternForward4 : public smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSPatternLoopStart>, + Transition, StiSPatternRotate4> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() + { + //auto &superstate = this->context(); + + this->configure(SS::pitch2_lenght_meters()); + this->configure(); + this->configure(); + } +}; +} // namespace s_pattern_states +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_loop_start.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_loop_start.hpp new file mode 100644 index 000000000..3b6a0f7c1 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_loop_start.hpp @@ -0,0 +1,60 @@ +// 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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_ue +{ +namespace s_pattern_states +{ +template +struct B : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + //typedef typename TDerived::reactions reactions; +}; + +// STATE DECLARATION +struct StiSPatternLoopStart : public B +{ + using B::B; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSPatternRotate1, CONTINUELOOP> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} + + bool loopCondition() + { + auto & superstate = this->context(); + return superstate.iteration_count++ < superstate.total_iterations(); + } + + void onEntry() { checkWhileLoopConditionAndThrowEvent(&StiSPatternLoopStart::loopCondition); } +}; + +} // namespace s_pattern_states +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_rotate_1.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_rotate_1.hpp new file mode 100644 index 000000000..d672586b8 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_rotate_1.hpp @@ -0,0 +1,61 @@ +// 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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_ue +{ +namespace s_pattern_states +{ +// STATE DECLARATION +struct StiSPatternRotate1 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSPatternForward1>, + Transition, StiSPatternRotate1> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() + { + auto & superstate = this->context(); + + float offset = 0; + if (superstate.direction() == TDirection::RIGHT) + { + // - offset because we are looking to the north and we have to turn clockwise + this->configure(0 - offset); + } + else + { + // - offset because we are looking to the south and we have to turn counter-clockwise + this->configure(180 + offset); + } + + this->configure(); + } +}; +} // namespace s_pattern_states +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_rotate_2.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_rotate_2.hpp new file mode 100644 index 000000000..e251df95f --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_rotate_2.hpp @@ -0,0 +1,62 @@ +// 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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_ue +{ +namespace s_pattern_states +{ +// STATE DECLARATION +struct StiSPatternRotate2 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSPatternForward2>, + Transition, StiSPatternForward2> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + float offset = 0; + float angle = 0; + if (SS::direction() == TDirection::LEFT) + angle = 90 + offset; + else + angle = -90 - offset; + + configure_orthogonal(angle); + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() + { + auto & superstate = this->context(); + RCLCPP_INFO( + getLogger(), "[StiSPatternRotate] SpatternRotate rotate: SS current iteration: %d/%d", + superstate.iteration_count, SS::total_iterations()); + } +}; +} // namespace s_pattern_states +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_rotate_3.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_rotate_3.hpp new file mode 100644 index 000000000..ef78446c0 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_rotate_3.hpp @@ -0,0 +1,73 @@ +// 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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_ue +{ +namespace s_pattern_states +{ +// STATE DECLARATION +struct StiSPatternRotate3 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSPatternForward3>, + Transition, StiSPatternRotate3> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() + { + auto & superstate = this->context(); + RCLCPP_INFO( + getLogger(), "[StiSPatternRotate] SpatternRotate rotate: SS current iteration: %d/%d", + superstate.iteration_count, SS::total_iterations()); + + float offset = 0; + // float angle = 0; + // if (superstate.direction() == TDirection::LEFT) + // angle = -90 - offset; + // else + // angle = +90 + offset; + + // this->configure(angle); + + if (superstate.direction() == TDirection::RIGHT) + { + // - offset because we are looking to the north and we have to turn clockwise + this->configure(0 - offset); + } + else + { + // - offset because we are looking to the south and we have to turn counter-clockwise + this->configure(180 + offset); + } + + this->configure(); + this->configure(); + } +}; +} // namespace s_pattern_states +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_rotate_4.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_rotate_4.hpp new file mode 100644 index 000000000..dd5e29168 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_rotate_4.hpp @@ -0,0 +1,61 @@ +// 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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_ue +{ +namespace s_pattern_states +{ +// STATE DECLARATION +struct StiSPatternRotate4 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSPatternForward4>, + Transition, StiSPatternRotate4> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() + { + auto & superstate = this->context(); + RCLCPP_INFO( + getLogger(), "[SsrSPatternRotate] SpatternRotate rotate: SS current iteration: %d/%d", + superstate.iteration_count, SS::total_iterations()); + + float offset = 0; + float angle = 0; + if (superstate.direction() == TDirection::LEFT) + angle = -90 - offset; + else + angle = 90 + offset; + + this->configure(angle); + this->configure(); + this->configure(); + } +}; +} // namespace s_pattern_states +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_acquire_sensors.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_acquire_sensors.hpp new file mode 100644 index 000000000..b1f9713ec --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_acquire_sensors.hpp @@ -0,0 +1,65 @@ +// 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 + +namespace sm_dancebot_ue +{ +using namespace smacc2::default_events; + +// STATE DECLARATION +struct StAcquireSensors : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct ON_SENSORS_AVAILABLE : SUCCESS{}; + struct SrAcquireSensors; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StEventCountDown, ON_SENSORS_AVAILABLE>, + Transition + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(); + configure_orthogonal("Hello World!"); + configure_orthogonal(); + configure_orthogonal(Service3Command::SERVICE3_ON); + configure_orthogonal(); + configure_orthogonal(); + + // Create State Reactor + auto srAllSensorsReady = static_createStateReactor< + SrAllEventsGo, smacc2::state_reactors::EvAllGo, + mpl::list< + EvTopicMessage, + EvTopicMessage, + EvCbSuccess>>(); + } +}; +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_event_count_down.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_event_count_down.hpp new file mode 100644 index 000000000..4c40cf7c4 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_event_count_down.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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#pragma once + +#include + +namespace sm_dancebot_ue +{ +// STATE DECLARATION +struct StEventCountDown : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StNavigateToWaypointsX>, + Transition + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + // configure_orthogonal(); + // configure_orthogonal("Hello World!"); + // configure_orthogonal(); + // configure_orthogonal(Service3Command::SERVICE3_ON); + + // Create State Reactor + + //auto srCountdown = static_createStateReactor(5); + //srCountdown->addInputEvent>(); + //srCountdown->setOutputEvent>(); + + auto srCountdown = static_createStateReactor< + SrEventCountdown, EvCountdownEnd, mpl::list>>(5); + } +}; +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_forward_1.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_forward_1.hpp new file mode 100644 index 000000000..f985bc858 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_forward_1.hpp @@ -0,0 +1,70 @@ +// 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 + +namespace sm_dancebot_ue +{ +// STATE DECLARATION +struct StNavigateForward1 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StRotateDegrees2>, + Transition, StNavigateForward1, ABORT> + //, Transition, StNavigateToWaypointsX, PREEMPT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(1); + configure_orthogonal(); + configure_orthogonal(); + configure_orthogonal(); + + } + + void runtimeConfigure() + { + ClNav2Z * move_base_action_client; + this->requiresClient(move_base_action_client); + + // we careful with the lifetime of the callbac, us a scoped connection if is not forever + move_base_action_client->onSucceeded(&StNavigateForward1::onActionClientSucceeded, this); + } + + void onActionClientSucceeded(cl_nav2z::ClNav2Z::WrappedResult & msg) + { + RCLCPP_INFO_STREAM( + getLogger(), + " [Callback SmaccSignal] Success Detected from StAcquireSensors (connected to client signal), " + "result data: " + << msg.result); + } +}; +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_forward_2.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_forward_2.hpp new file mode 100644 index 000000000..4f0e5f80e --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_forward_2.hpp @@ -0,0 +1,39 @@ +// 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. + +#include +namespace sm_dancebot_ue +{ +// STATE DECLARATION +struct StNavigateForward2 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StRotateDegrees5>, + Transition, StNavigateToWaypointsX> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(1); + configure_orthogonal(); + configure_orthogonal(); + } +}; +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_reverse_1.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_reverse_1.hpp new file mode 100644 index 000000000..1033ed3bd --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_reverse_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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#pragma once + +#include + +namespace sm_dancebot_ue +{ +// STATE DECLARATION +struct StNavigateReverse1 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StRotateDegrees3>, + Transition, StNavigateReverse1> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(1); + configure_orthogonal(); + configure_orthogonal(); + configure_orthogonal(); + } +}; +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_reverse_2.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_reverse_2.hpp new file mode 100644 index 000000000..71ca57dbb --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_reverse_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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#pragma once + +#include + +namespace sm_dancebot_ue +{ +// STATE DECLARATION +struct StNavigateReverse2 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StNavigateToWaypointsX>, + Transition, StNavigateToWaypointsX> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(1); + configure_orthogonal(); + configure_orthogonal(); + configure_orthogonal(); + } +}; +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_reverse_3.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_reverse_3.hpp new file mode 100644 index 000000000..51be066bd --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_reverse_3.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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#pragma once + +#include + +namespace sm_dancebot_ue +{ +// STATE DECLARATION +struct StNavigateReverse3 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StNavigateToWaypoint1>, + Transition, StNavigateToWaypointsX> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(1); + configure_orthogonal(); + configure_orthogonal(); + configure_orthogonal(); + } +}; +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_reverse_4.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_reverse_4.hpp new file mode 100644 index 000000000..302f14f61 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_reverse_4.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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#pragma once + +#include + +namespace sm_dancebot_ue +{ +// STATE DECLARATION +struct StNavigateReverse4 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StRotateDegrees5>, + Transition, StNavigateToWaypointsX> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(0.5); + configure_orthogonal(); + configure_orthogonal(); + configure_orthogonal(); + } +}; +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_to_waypoint_1.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_to_waypoint_1.hpp new file mode 100644 index 000000000..1e24548e9 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_to_waypoint_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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#pragma once + +#include + +namespace sm_dancebot_ue +{ +// STATE DECLARATION +struct StNavigateToWaypoint1 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StNavigateToWaypointsX>, + Transition, StNavigateToWaypoint1> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(2.0, 0, 0); + configure_orthogonal(); + configure_orthogonal("All Done!"); + configure_orthogonal(); + } +}; +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_to_waypoints_x.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_to_waypoints_x.hpp new file mode 100644 index 000000000..075f9fc15 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_to_waypoints_x.hpp @@ -0,0 +1,72 @@ +// 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 + +namespace sm_dancebot_ue +{ +// STATE DECLARATION +struct StNavigateToWaypointsX : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // CUSTOM TRANSITION TAGS + struct TRANSITION_1 : SUCCESS{}; + struct TRANSITION_2 : SUCCESS{}; + struct TRANSITION_3 : SUCCESS{}; + struct TRANSITION_4 : SUCCESS{}; + struct TRANSITION_5 : SUCCESS{}; + struct TRANSITION_6 : SUCCESS{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, SS1::SsRadialPattern1, TRANSITION_1>, + Transition, SS2::SsRadialPattern2, TRANSITION_2>, + Transition, SS3::SsRadialPattern3, TRANSITION_3>, + Transition, SS4::SsFPattern1, TRANSITION_4>, + Transition, StNavigateToWaypointsX, TRANSITION_5>, + Transition, SS5::SsSPattern1, TRANSITION_6>, + Transition, StNavigateToWaypointsX, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(); + configure_orthogonal(); + configure_orthogonal(); + configure_orthogonal(); + } + + void onEntry() + { + } + + void runtimeConfigure() {} + + void onExit(ABORT) + { + } +}; +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_rotate_degrees_1.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_rotate_degrees_1.hpp new file mode 100644 index 000000000..69dffce91 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_rotate_degrees_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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#pragma once + +#include + +namespace sm_dancebot_ue +{ +// STATE DECLARATION +struct StRotateDegrees1 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StNavigateForward1>, + Transition, StNavigateToWaypointsX> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(/*30*/ 90); + configure_orthogonal(); + configure_orthogonal(); + configure_orthogonal(); + } +}; +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_rotate_degrees_2.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_rotate_degrees_2.hpp new file mode 100644 index 000000000..b8631ae63 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_rotate_degrees_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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#pragma once + +#include +namespace sm_dancebot_ue +{ +// STATE DECLARATION +struct StRotateDegrees2 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StNavigateToWaypointsX>, + Transition, StNavigateToWaypointsX> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(/*30*/ -90); + configure_orthogonal(); + configure_orthogonal(); + configure_orthogonal(); + } +}; +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_rotate_degrees_3.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_rotate_degrees_3.hpp new file mode 100644 index 000000000..ecc48df26 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_rotate_degrees_3.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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#pragma once + +#include + +namespace sm_dancebot_ue +{ +// STATE DECLARATION +struct StRotateDegrees3 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StNavigateToWaypointsX>, + Transition, StNavigateToWaypointsX> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(/*30*/ 180); + configure_orthogonal(); + configure_orthogonal(); + configure_orthogonal(); + } +}; +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_rotate_degrees_4.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_rotate_degrees_4.hpp new file mode 100644 index 000000000..1060dcd35 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_rotate_degrees_4.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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#pragma once + +#include +namespace sm_dancebot_ue +{ +// STATE DECLARATION +struct StRotateDegrees4 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StNavigateReverse2>, + Transition, StNavigateToWaypointsX> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(/*30*/ -180); + configure_orthogonal(); + configure_orthogonal(); + configure_orthogonal(); + } +}; +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_rotate_degrees_5.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_rotate_degrees_5.hpp new file mode 100644 index 000000000..9d2c0d57b --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_rotate_degrees_5.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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#pragma once + +#include +namespace sm_dancebot_ue +{ +// STATE DECLARATION +struct StRotateDegrees5 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StNavigateToWaypointsX>, + Transition, StNavigateToWaypointsX> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(/*30*/ -180); + configure_orthogonal(); + configure_orthogonal(); + configure_orthogonal(); + } +}; +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_rotate_degrees_6.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_rotate_degrees_6.hpp new file mode 100644 index 000000000..85ba0feed --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_rotate_degrees_6.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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#pragma once + +#include +namespace sm_dancebot_ue +{ +// STATE DECLARATION +struct StRotateDegrees6 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StNavigateReverse3>, + Transition, StNavigateToWaypointsX> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(/*30*/ -180); + configure_orthogonal(); + configure_orthogonal(); + configure_orthogonal(); + } +}; +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_f_pattern_1.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_f_pattern_1.hpp new file mode 100644 index 000000000..eefc25161 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_f_pattern_1.hpp @@ -0,0 +1,92 @@ +// 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 + * + ******************************************************************************************************************/ + +#include + +namespace sm_dancebot_ue +{ +namespace f_pattern_states +{ +enum class TDirection +{ + LEFT, + RIGHT +}; + +// FORWARD DECLARATIONS OF INNER STATES +template class StiFPatternRotate1; +template class StiFPatternForward1; +template class StiFPatternReturn1; +template class StiFPatternRotate2; +template class StiFPatternForward2; +template class StiFPatternStartLoop; + +} // namespace f_pattern_states +} // namespace sm_dancebot_ue +namespace sm_dancebot_ue +{ +namespace SS4 +{ +using namespace f_pattern_states; + +// STATE DECLARATION +struct SsFPattern1 +: smacc2::SmaccState> +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition>, StNavigateReverse4, ENDLOOP> //, + + >reactions; + + // STATE VARIABLES + // superstate parameters + static constexpr float ray_lenght_meters() { return 3.25; } + static constexpr float pitch_lenght_meters() { return 0.75; } + static constexpr int total_iterations() { return 10; } + static constexpr TDirection direction() { return TDirection::RIGHT; } + + // superstate state variables + int iteration_count; + + // STATE FUNCTIONS + static void staticConfigure() + { + //configure_orthogonal(); + } + + void runtimeConfigure() { iteration_count = 0; } +}; // namespace SS4 + +// FORWARD DECLARATION FOR THE SUPERSTATE + +} // namespace SS4 +} // namespace sm_dancebot_ue + +#include +#include +#include +#include +#include +#include diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_radial_pattern_1.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_radial_pattern_1.hpp new file mode 100644 index 000000000..92df50fd7 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_radial_pattern_1.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 + * + ******************************************************************************************************************/ + +#include + +namespace sm_dancebot_ue +{ +namespace SS1 +{ +namespace sm_dancebot_ue +{ +namespace radial_motion_states +{ +//FORWARD DECLARATION OF INNER STATES +class StiRadialRotate; +class StiRadialReturn; +class StiRadialEndPoint; +class StiRadialLoopStart; + +} // namespace radial_motion_states +} // namespace sm_dancebot_ue +using namespace sm_dancebot_ue::radial_motion_states; + +// STATE DECLARATION +struct SsRadialPattern1 +: smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + // Transition, StRotateDegrees1, ENDLOOP> + Transition, StNavigateReverse1, ENDLOOP> + + >reactions; + + static constexpr int total_iterations() { return 16; } + static constexpr float ray_angle_increment_degree() { return 360.0 / total_iterations(); } + static constexpr float ray_length_meters() { return 4; } + + int iteration_count = 0; + + // STATE FUNCTIONS + static void staticConfigure() + { + //configure_orthogonal(); + } + + void runtimeConfigure() {} +}; + +// FORWARD DECLARATION FOR THE SUPERSTATE +using SS = SsRadialPattern1; +#include +#include +#include +#include +} // namespace SS1 +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_radial_pattern_2.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_radial_pattern_2.hpp new file mode 100644 index 000000000..ba57e72d9 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_radial_pattern_2.hpp @@ -0,0 +1,81 @@ +// 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 + * + ******************************************************************************************************************/ + +#include + +namespace sm_dancebot_ue +{ +namespace SS2 +{ +namespace sm_dancebot_ue +{ +namespace radial_motion_states +{ +// FORWARD DECLARATIONS OF INNER STATES +class StiRadialRotate; +class StiRadialReturn; +class StiRadialEndPoint; +class StiRadialLoopStart; +} // namespace radial_motion_states +} // namespace sm_dancebot_ue +using namespace sm_dancebot_ue::radial_motion_states; + +// STATE DECLARATION +struct SsRadialPattern2 +: smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StNavigateReverse1, ENDLOOP> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + //configure_orthogonal(); + } + + void runtimeConfigure() {} + + int iteration_count = 0; + static constexpr int total_iterations() { return 16; } + static constexpr float ray_angle_increment_degree() { return 360.0 / total_iterations(); } + static constexpr float ray_length_meters() { return 5; } + + void onExit() + { + rclcpp::sleep_for(5s); + RCLCPP_INFO(getLogger(), "[SsRadialPattern1] waiting 5 seconds"); + } +}; + +// FORWARD DECLARATION FOR THE SUPERSTATE +using SS = SsRadialPattern2; +#include +#include +#include +#include +} // namespace SS2 +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_radial_pattern_3.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_radial_pattern_3.hpp new file mode 100644 index 000000000..2f1754f5a --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_radial_pattern_3.hpp @@ -0,0 +1,76 @@ +// 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 + * + ******************************************************************************************************************/ + +#include + +namespace sm_dancebot_ue +{ +namespace SS3 +{ +namespace sm_dancebot_ue +{ +namespace radial_motion_states +{ +// FORWARD DECLARATION OF INNER STATES +class StiRadialRotate; +class StiRadialReturn; +class StiRadialEndPoint; +class StiRadialLoopStart; +} // namespace radial_motion_states +} // namespace sm_dancebot_ue +using namespace sm_dancebot_ue::radial_motion_states; + +// STATE DECLARATION +struct SsRadialPattern3 +: smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StRotateDegrees4, ENDLOOP> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + //configure_orthogonal(); + } + + int iteration_count; + + static constexpr int total_iterations() { return 16; } + static constexpr float ray_angle_increment_degree() { return 360.0 / total_iterations(); } + static constexpr float ray_length_meters() { return 4; } + + void runtimeConfigure() { iteration_count = 0; } +}; + +// FORWARD DECLARATION FOR THE SUPERSTATE +using SS = SsRadialPattern3; +#include +#include +#include +#include +} // namespace SS3 +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_s_pattern_1.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_s_pattern_1.hpp new file mode 100644 index 000000000..538d22b82 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_s_pattern_1.hpp @@ -0,0 +1,93 @@ +// 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 + * + ******************************************************************************************************************/ + +#include + +namespace sm_dancebot_ue +{ +namespace SS5 +{ +namespace sm_dancebot_ue +{ +namespace s_pattern_states +{ +// FORWARD DECLARATIONS OF INNER STATES +class StiSPatternRotate1; +class StiSPatternForward1; +class StiSPatternRotate2; +class StiSPatternForward2; +class StiSPatternRotate3; +class StiSPatternForward3; +class StiSPatternRotate4; +class StiSPatternForward4; +class StiSPatternLoopStart; +} // namespace s_pattern_states +} // namespace sm_dancebot_ue + +enum class TDirection +{ + LEFT, + RIGHT +}; + +using namespace sm_dancebot_ue::s_pattern_states; + +// STATE DECLARATION +struct SsSPattern1 : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StNavigateReverse3, ENDLOOP> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + //configure_orthogonal(); + } + + static constexpr float pitch1_lenght_meters() { return 0.75; } + static constexpr float pitch2_lenght_meters() { return 1.45; } + static constexpr int total_iterations() { return 9; } + static constexpr TDirection direction() { return TDirection::RIGHT; } + + int iteration_count; + + void runtimeConfigure() { this->iteration_count = 0; } +}; + +// FORWARD DECLARATION FOR THE SUPERSTATE +using SS = SsSPattern1; +#include +#include +#include +#include +#include +#include +#include +#include +#include +} // namespace SS5 +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/launch/bringup_launch.py b/smacc2_sm_reference_library/sm_dancebot_ue/launch/bringup_launch.py new file mode 100644 index 000000000..1714986e4 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/launch/bringup_launch.py @@ -0,0 +1,137 @@ +# 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 ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + GroupAction, + IncludeLaunchDescription, + SetEnvironmentVariable, +) +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.actions import PushRosNamespace + + +def generate_launch_description(): + # Get the launch directory + sm_dance_bot_dir = get_package_share_directory("sm_dance_bot") + launch_dir = os.path.join(sm_dance_bot_dir, "launch") + + # Create the launch configuration variables + namespace = LaunchConfiguration("namespace") + use_namespace = LaunchConfiguration("use_namespace") + slam = LaunchConfiguration("slam") + map_yaml_file = LaunchConfiguration("map") + use_sim_time = LaunchConfiguration("use_sim_time") + params_file = LaunchConfiguration("params_file") + default_nav_to_pose_bt_xml = LaunchConfiguration("default_nav_to_pose_bt_xml") + autostart = LaunchConfiguration("autostart") + + stdout_linebuf_envvar = SetEnvironmentVariable("RCUTILS_LOGGING_BUFFERED_STREAM", "1") + + declare_namespace_cmd = DeclareLaunchArgument( + "namespace", default_value="", description="Top-level namespace" + ) + + declare_use_namespace_cmd = DeclareLaunchArgument( + "use_namespace", + default_value="false", + description="Whether to apply a namespace to the navigation stack", + ) + + declare_slam_cmd = DeclareLaunchArgument( + "slam", default_value="true", description="Whether run a SLAM" + ) + + declare_map_yaml_cmd = DeclareLaunchArgument( + "map", description="Full path to map yaml file to load" + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + "use_sim_time", default_value="false", description="Use simulation (Gazebo) clock if true" + ) + + declare_params_file_cmd = DeclareLaunchArgument( + "params_file", + default_value=os.path.join(sm_dance_bot_dir, "params", "nav2z_client", "nav2_params.yaml"), + description="Full path to the ROS2 parameters file to use for all launched nodes", + ) + + declare_bt_xml_cmd = DeclareLaunchArgument( + "default_nav_to_pose_bt_xml", + default_value=os.path.join( + sm_dance_bot_dir, "params", "nav2z_client", "navigation_tree.xml" + ), + description="Full path to the behavior tree xml file to use", + ) + + declare_autostart_cmd = DeclareLaunchArgument( + "autostart", default_value="true", description="Automatically startup the nav2 stack" + ) + + # Specify the actions + bringup_cmd_group = GroupAction( + [ + PushRosNamespace(condition=IfCondition(use_namespace), namespace=namespace), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(launch_dir, "slam_launch.py")), + condition=IfCondition(slam), + launch_arguments={ + "namespace": namespace, + "use_sim_time": use_sim_time, + "autostart": autostart, + "params_file": params_file, + }.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(launch_dir, "navigation_launch.py")), + launch_arguments={ + "namespace": namespace, + "use_sim_time": use_sim_time, + "autostart": autostart, + "params_file": params_file, + "default_nav_to_pose_bt_xml": default_nav_to_pose_bt_xml, + "use_lifecycle_mgr": "false", + "map_subscribe_transient_local": "true", + }.items(), + ), + ] + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Set environment variables + ld.add_action(stdout_linebuf_envvar) + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_namespace_cmd) + ld.add_action(declare_slam_cmd) + ld.add_action(declare_map_yaml_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_autostart_cmd) + ld.add_action(declare_bt_xml_cmd) + + # Add the actions to launch all of the navigation nodes + ld.add_action(bringup_cmd_group) + + return ld diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/launch/husky_gazebo.launch.yaml b/smacc2_sm_reference_library/sm_dancebot_ue/launch/husky_gazebo.launch.yaml new file mode 100644 index 000000000..66bb498b0 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/launch/husky_gazebo.launch.yaml @@ -0,0 +1,3 @@ +launch: + - include: + file: "$(find-pkg-share husky_gazebo)/launch/gazebo_launch.py" diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/launch/navigation_launch.py b/smacc2_sm_reference_library/sm_dancebot_ue/launch/navigation_launch.py new file mode 100644 index 000000000..22e8eeeb9 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/launch/navigation_launch.py @@ -0,0 +1,169 @@ +# 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 ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + # Get the launch directory + sm_dance_bot_dir = get_package_share_directory("sm_dance_bot") + + namespace = LaunchConfiguration("namespace") + use_sim_time = LaunchConfiguration("use_sim_time") + autostart = LaunchConfiguration("autostart") + params_file = LaunchConfiguration("params_file") + default_nav_to_pose_bt_xml = LaunchConfiguration("default_nav_to_pose_bt_xml") + map_subscribe_transient_local = LaunchConfiguration("map_subscribe_transient_local") + + lifecycle_nodes = ["controller_server", "planner_server", "behavior_server", "bt_navigator"] + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + # TODO(orduno) Substitute with `PushNodeRemapping` + # https://github.com/ros2/launch_ros/issues/56 + remappings = [("/tf", "tf"), ("/tf_static", "tf_static")] + + # Create our own temporary YAML files that include substitutions + param_substitutions = { + "use_sim_time": use_sim_time, + # 'default_nav_to_pose_bt_xml': default_nav_to_pose_bt_xml, + "default_nav_to_pose_bt_xml": os.path.join( + sm_dance_bot_dir, "params", "nav2z_client", "navigation_tree.xml" + ), + "autostart": autostart, + "map_subscribe_transient_local": map_subscribe_transient_local, + } + + configured_params = RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites=param_substitutions, + convert_types=True, + ) + + xtermprefix = "xterm -xrm 'XTerm*scrollBar: true' -xrm 'xterm*rightScrollBar: true' -hold -geometry 1000x600 -sl 10000 -e" + + print("+********************************") + print(str(param_substitutions)) + print(str(default_nav_to_pose_bt_xml)) + + return LaunchDescription( + [ + # Set env var to print messages to stdout immediately + SetEnvironmentVariable("RCUTILS_LOGGING_BUFFERED_STREAM", "1"), + DeclareLaunchArgument( + "namespace", default_value="", description="Top-level namespace" + ), + DeclareLaunchArgument( + "use_sim_time", + default_value="false", + description="Use simulation (Gazebo) clock if true", + ), + DeclareLaunchArgument( + "autostart", + default_value="true", + description="Automatically startup the nav2 stack", + ), + DeclareLaunchArgument( + "params_file", + default_value=os.path.join( + sm_dance_bot_dir, "params", "nav2z_client", "nav2_params.yaml" + ), + description="Full path to the ROS2 parameters file to use", + ), + DeclareLaunchArgument( + "default_nav_to_pose_bt_xml", + # default_value=os.path.join(get_package_share_directory('nav2_bt_navigator'),'behavior_trees', 'navigate_w_replanning_and_recovery.xml'), + default_value=os.path.join( + sm_dance_bot_dir, "params", "nav2z_client", "navigation_tree.xml" + ), + description="Full path to the behavior tree xml file to use", + ), + DeclareLaunchArgument( + "map_subscribe_transient_local", + default_value="false", + description="Whether to set the map subscriber QoS to transient local", + ), + Node( + package="nav2_controller", + executable="controller_server", + output="screen", + prefix=xtermprefix, + parameters=[configured_params], + remappings=remappings, + arguments=["--ros-args", "--log-level", "INFO"], + ), + Node( + package="nav2_planner", + executable="planner_server", + name="planner_server", + output="screen", + prefix=xtermprefix, + parameters=[configured_params], + remappings=remappings, + arguments=["--ros-args", "--log-level", "INFO"], + ), + Node( + package="nav2_behaviors", + executable="behavior_server", + name="behavior_server", + output="screen", + prefix=xtermprefix, + parameters=[configured_params], + remappings=remappings, + arguments=["--ros-args", "--log-level", "INFO"], + ), + Node( + package="nav2_bt_navigator", + executable="bt_navigator", + name="bt_navigator", + output="screen", + prefix=xtermprefix, + parameters=[configured_params], + remappings=remappings, + arguments=["--ros-args", "--log-level", "INFO"], + ), + Node( + package="nav2_waypoint_follower", + executable="waypoint_follower", + name="waypoint_follower", + output="screen", + parameters=[configured_params], + remappings=remappings, + ), + Node( + package="nav2_lifecycle_manager", + executable="lifecycle_manager", + name="lifecycle_manager_navigation", + output="screen", + prefix=xtermprefix, + parameters=[ + {"use_sim_time": use_sim_time}, + {"autostart": autostart}, + {"node_names": lifecycle_nodes}, + ], + arguments=["--ros-args", "--log-level", "INFO"], + ), + ] + ) diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/launch/online_sync_launch.py b/smacc2_sm_reference_library/sm_dancebot_ue/launch/online_sync_launch.py new file mode 100644 index 000000000..054d891fd --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/launch/online_sync_launch.py @@ -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. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + use_sim_time = LaunchConfiguration("use_sim_time") + + declare_use_sim_time_argument = DeclareLaunchArgument( + "use_sim_time", default_value="true", description="Use simulation/Gazebo clock" + ) + + xtermprefix = "xterm -xrm 'XTerm*scrollBar: true' -xrm 'xterm*rightScrollBar: true' -hold -geometry 1000x600 -sl 10000 -e" + + start_sync_slam_toolbox_node = Node( + parameters=[ + get_package_share_directory("slam_toolbox") + "/config/mapper_params_online_sync.yaml", + {"use_sim_time": use_sim_time}, + ], + package="slam_toolbox", + executable="sync_slam_toolbox_node", + name="slam_toolbox", + output="screen", + prefix=xtermprefix, + ) + + ld = LaunchDescription() + + ld.add_action(declare_use_sim_time_argument) + ld.add_action(start_sync_slam_toolbox_node) + + return ld diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/launch/rviz_launch.py b/smacc2_sm_reference_library/sm_dancebot_ue/launch/rviz_launch.py new file mode 100644 index 000000000..056b5f8f0 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/launch/rviz_launch.py @@ -0,0 +1,122 @@ +# 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 ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, EmitEvent, RegisterEventHandler +from launch.conditions import IfCondition, UnlessCondition +from launch.event_handlers import OnProcessExit +from launch.events import Shutdown +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from nav2_common.launch import ReplaceString + + +def generate_launch_description(): + # Get the launch directory + bringup_dir = get_package_share_directory("nav2_bringup") + + # Create the launch configuration variables + namespace = LaunchConfiguration("namespace") + use_namespace = LaunchConfiguration("use_namespace") + rviz_config_file = LaunchConfiguration("rviz_config") + + # Declare the launch arguments + declare_namespace_cmd = DeclareLaunchArgument( + "namespace", + default_value="navigation", + description=( + "Top-level namespace. The value will be used to replace the " + " keyword on the rviz config file." + ), + ) + + declare_use_namespace_cmd = DeclareLaunchArgument( + "use_namespace", + default_value="false", + description="Whether to apply a namespace to the navigation stack", + ) + + declare_rviz_config_file_cmd = DeclareLaunchArgument( + "rviz_config", + default_value=os.path.join(bringup_dir, "rviz", "nav2_default_view.rviz"), + description="Full path to the RVIZ config file to use", + ) + + # Launch rviz + start_rviz_cmd = Node( + condition=UnlessCondition(use_namespace), + package="rviz2", + executable="rviz2", + name="rviz2", + arguments=["-d", rviz_config_file], + output="screen", + ) + + namespaced_rviz_config_file = ReplaceString( + source_file=rviz_config_file, replacements={"": ("/", namespace)} + ) + + start_namespaced_rviz_cmd = Node( + condition=IfCondition(use_namespace), + package="rviz2", + executable="rviz2", + name="rviz2", + namespace=namespace, + arguments=["-d", namespaced_rviz_config_file], + output="screen", + remappings=[ + ("/tf", "tf"), + ("/tf_static", "tf_static"), + ("/goal_pose", "goal_pose"), + ("/clicked_point", "clicked_point"), + ("/initialpose", "initialpose"), + ], + ) + + exit_event_handler = RegisterEventHandler( + condition=UnlessCondition(use_namespace), + event_handler=OnProcessExit( + target_action=start_rviz_cmd, on_exit=EmitEvent(event=Shutdown(reason="rviz exited")) + ), + ) + + exit_event_handler_namespaced = RegisterEventHandler( + condition=IfCondition(use_namespace), + event_handler=OnProcessExit( + target_action=start_namespaced_rviz_cmd, + on_exit=EmitEvent(event=Shutdown(reason="rviz exited")), + ), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_namespace_cmd) + ld.add_action(declare_rviz_config_file_cmd) + + # Add any conditioned actions + ld.add_action(start_rviz_cmd) + ld.add_action(start_namespaced_rviz_cmd) + + # Add other nodes and processes we need + ld.add_action(exit_event_handler) + ld.add_action(exit_event_handler_namespaced) + + return ld diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/launch/slam_launch.py b/smacc2_sm_reference_library/sm_dancebot_ue/launch/slam_launch.py new file mode 100644 index 000000000..a3d1aa210 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/launch/slam_launch.py @@ -0,0 +1,128 @@ +# Copyright (c) 2020 Samsung Research Russia +# +# 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 ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + +# from launch_ros.actions import Node +# from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + # Input parameters declaration + # namespace = LaunchConfiguration("namespace") + # params_file = LaunchConfiguration("params_file") + use_sim_time = LaunchConfiguration("use_sim_time") + # autostart = LaunchConfiguration("autostart") + # sm_dance_bot_dir = get_package_share_directory("sm_dance_bot") + + # Variables + # lifecycle_nodes = ["slam_toolbox"] + + # Getting directories and launch-files + # bringup_dir = get_package_share_directory("nav2_bringup") + slam_toolbox_dir = get_package_share_directory("slam_toolbox") + # slam_launch_file = os.path.join(sm_dance_bot_dir, 'launch', 'online_sync_launch.py') + slam_launch_file = os.path.join(slam_toolbox_dir, "launch", "online_sync_launch.py") + + # Create our own temporary YAML files that include substitutions + # param_substitutions = {"use_sim_time": use_sim_time} + + # configured_params = RewrittenYaml( + # source_file=params_file, + # root_key=namespace, + # param_rewrites=param_substitutions, + # convert_types=True, + # ) + + # Declare the launch arguments + declare_namespace_cmd = DeclareLaunchArgument( + "namespace", default_value="", description="Top-level namespace" + ) + + # declare_params_file_cmd = DeclareLaunchArgument( + # 'params_file', + # default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), + # description='Full path to the ROS2 parameters file to use for all launched nodes') + + declare_use_sim_time_cmd = DeclareLaunchArgument( + "use_sim_time", default_value="True", description="Use simulation (Gazebo) clock if true" + ) + + declare_autostart_cmd = DeclareLaunchArgument( + "autostart", default_value="True", description="Automatically startup the nav2 stack" + ) + + # Nodes launching commands + + # start_map_saver_server_cmd = Node( + # package='nav2_map_server', + # executable='map_saver_server', + # output='screen', + # parameters=[configured_params]) + + # xtermprefix = "xterm -xrm 'XTerm*scrollBar: true' -xrm 'xterm*rightScrollBar: true' " \ + # "-hold -geometry 1000x600 -sl 10000 -e" + + # start_lifecycle_manager_cmd = Node( + # package="nav2_lifecycle_manager", + # executable="lifecycle_manager", + # name="lifecycle_manager_slam", + # output="screen", + # parameters=[ + # {"use_sim_time": use_sim_time}, + # {"autostart": autostart}, + # {"node_names": lifecycle_nodes}, + # ], + # prefix=xtermprefix, + # ) + + # If the provided param file doesn't have slam_toolbox params, we must remove the 'params_file' + # LaunchConfiguration, or it will be passed automatically to slam_toolbox and will not load + # the default file + + start_slam_toolbox_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource(slam_launch_file), + launch_arguments={"use_sim_time": use_sim_time}.items(), + ) + + # Pop (or load) previous LaunchConfiguration, resetting the state of params_file + # pop_launch_config = PopLaunchConfigurations( + # condition=UnlessCondition(has_slam_toolbox_params)) + + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + # ld.add_action(declare_params_file_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_autostart_cmd) + + # Running Map Saver Server + # ld.add_action(start_map_saver_server_cmd) + # ld.add_action(start_lifecycle_manager_cmd) + + # Running SLAM Toolbox + # ld.add_action(push_launch_config) + # ld.add_action(remove_params_file) + ld.add_action(start_slam_toolbox_cmd) + # ld.add_action(pop_launch_config) + + return ld diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/launch/sm_dance_bot_launch.py b/smacc2_sm_reference_library/sm_dancebot_ue/launch/sm_dance_bot_launch.py new file mode 100644 index 000000000..6e30a5641 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/launch/sm_dance_bot_launch.py @@ -0,0 +1,252 @@ +# 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. + +"""This is all-in-one launch script intended for use by nav2 developers.""" + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + # Get the launch directory + sm_dance_bot_dir = get_package_share_directory("sm_dance_bot") + sm_dance_bot_launch_dir = os.path.join(sm_dance_bot_dir, "launch") + + # Create the launch configuration variables + slam = LaunchConfiguration("slam") + namespace = LaunchConfiguration("namespace") + use_namespace = LaunchConfiguration("use_namespace") + map_yaml_file = LaunchConfiguration("map") + use_sim_time = LaunchConfiguration("use_sim_time") + gazebo_headless = LaunchConfiguration("headless") + + params_file = LaunchConfiguration("params_file") + default_nav_to_pose_bt_xml = LaunchConfiguration("default_nav_to_pose_bt_xml") + autostart = LaunchConfiguration("autostart") + show_gz_lidar = LaunchConfiguration("show_gz_lidar") + + # Launch configuration variables specific to simulation + rviz_config_file = LaunchConfiguration("rviz_config_file") + + use_robot_state_pub = LaunchConfiguration("use_robot_state_pub") + use_rviz = LaunchConfiguration("use_rviz") + + urdf = os.path.join(sm_dance_bot_dir, "urdf", "turtlebot3_waffle.urdf") + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + # TODO(orduno) Substitute with `PushNodeRemapping` + # https://github.com/ros2/launch_ros/issues/56 + remappings = [("/tf", "tf"), ("/tf_static", "tf_static")] + + # Declare the launch arguments + declare_namespace_cmd = DeclareLaunchArgument( + "namespace", default_value="", description="Top-level namespace" + ) + + declare_use_namespace_cmd = DeclareLaunchArgument( + "use_namespace", + default_value="false", + description="Whether to apply a namespace to the navigation stack", + ) + + declare_show_gz_lidar = DeclareLaunchArgument( + "show_gz_lidar", + default_value="true", + description="Whether to apply a namespace to the navigation stack", + ) + + declare_slam_cmd = DeclareLaunchArgument( + "slam", default_value="True", description="Whether run a SLAM" + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + "use_sim_time", default_value="true", description="Use simulation (Gazebo) clock if true" + ) + + declare_gazebo_headless_cmd = DeclareLaunchArgument( + "headless", + default_value="false", + description="Use headless Gazebo if true", + ) + + declare_params_file_cmd = DeclareLaunchArgument( + "params_file", + default_value=os.path.join(sm_dance_bot_dir, "params", "nav2z_client", "nav2_params.yaml"), + description="Full path to the ROS2 parameters file to use for all launched nodes", + ) + + declare_bt_xml_cmd = DeclareLaunchArgument( + "default_nav_to_pose_bt_xml", + default_value=os.path.join( + sm_dance_bot_dir, "params", "nav2z_client", "navigation_tree.xml" + ), + description="Full path to the behavior tree xml file to use", + ) + + declare_autostart_cmd = DeclareLaunchArgument( + "autostart", default_value="true", description="Automatically startup the nav2 stack" + ) + + declare_rviz_config_file_cmd = DeclareLaunchArgument( + "rviz_config_file", + default_value=os.path.join(sm_dance_bot_dir, "rviz", "nav2_default_view.rviz"), + description="Full path to the RVIZ config file to use", + ) + + declare_use_robot_state_pub_cmd = DeclareLaunchArgument( + "use_robot_state_pub", + default_value="True", + description="Whether to start the robot state publisher", + ) + + declare_use_rviz_cmd = DeclareLaunchArgument( + "use_rviz", default_value="True", description="Whether to start RVIZ" + ) + + declare_map_yaml_cmd = DeclareLaunchArgument( + "map", + default_value=os.path.join(sm_dance_bot_dir, "maps", "turtlebot3_world.yaml"), + description="Full path to map file to load", + ) + + start_robot_state_publisher_cmd = Node( + condition=IfCondition(use_robot_state_pub), + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + namespace=namespace, + output="screen", + parameters=[{"use_sim_time": use_sim_time}], + remappings=remappings, + arguments=[urdf], + ) + + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(sm_dance_bot_launch_dir, "rviz_launch.py")), + condition=IfCondition(use_rviz), + launch_arguments={ + "namespace": "", + "use_namespace": "False", + "rviz_config": rviz_config_file, + }.items(), + ) + + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(sm_dance_bot_launch_dir, "bringup_launch.py")), + launch_arguments={ + "namespace": namespace, + "use_namespace": use_namespace, + "autostart": autostart, + "params_file": params_file, + "slam": slam, + "map": map_yaml_file, + "use_sim_time": use_sim_time, + "default_nav_to_pose_bt_xml": default_nav_to_pose_bt_xml, + }.items(), + ) + + gazebo_simulator = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(sm_dance_bot_launch_dir, "gazebo_launch.py")), + launch_arguments={"show_gz_lidar": show_gz_lidar, "headless": gazebo_headless}.items(), + ) + + xtermprefix = "xterm -xrm 'XTerm*scrollBar: true' -xrm 'xterm*rightScrollBar: true' -hold -geometry 1000x600 -sl 10000 -e" + + sm_dance_bot_node = Node( + package="sm_dance_bot", + executable="sm_dance_bot_node", + name="SmDanceBot", + output="screen", + prefix=xtermprefix, + parameters=[ + os.path.join( + get_package_share_directory("sm_dance_bot"), + "params/sm_dance_bot_config.yaml", + ) + ], + remappings=[ + # ("/odom", "/odometry/filtered"), + # ("/sm_dance_bot_2/odom_tracker/odom_tracker_path", "/odom_tracker_path"), + # ("/sm_dance_bot_2/odom_tracker/odom_tracker_stacked_path", "/odom_tracker_path_stacked") + ], + arguments=["--ros-args", "--log-level", "INFO"], + ) + + led_action_server_node = Node( + package="sm_dance_bot", + executable="led_action_server_node", + output="screen", + prefix=xtermprefix, + ) + + temperature_action_server = Node( + package="sm_dance_bot", + executable="temperature_sensor_node", + output="screen", + prefix=xtermprefix, + ) + + service3_node = Node( + package="sm_dance_bot", + executable="service_node_3.py", + output="screen", + prefix=xtermprefix, + parameters=[ + {"autostart": True, "node_names": ["ss", "dfa"]}, + ], + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_namespace_cmd) + ld.add_action(declare_slam_cmd) + ld.add_action(declare_map_yaml_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_gazebo_headless_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_bt_xml_cmd) + ld.add_action(declare_autostart_cmd) + ld.add_action(declare_map_yaml_cmd) + ld.add_action(declare_show_gz_lidar) + + ld.add_action(declare_rviz_config_file_cmd) + ld.add_action(declare_use_robot_state_pub_cmd) + ld.add_action(declare_use_rviz_cmd) + ld.add_action(gazebo_simulator) + + ld.add_action(sm_dance_bot_node) + ld.add_action(service3_node) + ld.add_action(temperature_action_server) + ld.add_action(led_action_server_node) + + # # Add the actions to launch all of the navigation nodes + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + + return ld diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/package.xml b/smacc2_sm_reference_library/sm_dancebot_ue/package.xml new file mode 100644 index 000000000..7fdf2487b --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/package.xml @@ -0,0 +1,44 @@ + + + + sm_dancebot_ue + 2.2.2 + The dance_bot package + Pablo Inigo Blasco + + Apache-2.0 + + ament_cmake + + nav2z_client + multirole_sensor_client + navigation2 + nav2_bringup + + robot_state_publisher + ros_publisher_client + ros_timer_client + slam_toolbox + smacc2 + std_msgs + sr_all_events_go + sr_event_countdown + sr_conditional + visualization_msgs + tf2 + + + gazebo_ros + gazebo + gazebo_plugins + + rosidl_default_generators + action_msgs + rosidl_interface_packages + rosidl_default_runtime + + + ament_cmake + + + diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/params/mapper_params_online_sync.yaml b/smacc2_sm_reference_library/sm_dancebot_ue/params/mapper_params_online_sync.yaml new file mode 100644 index 000000000..751672c49 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/params/mapper_params_online_sync.yaml @@ -0,0 +1,73 @@ +slam_toolbox: + ros__parameters: + + # Plugin params + solver_plugin: solver_plugins::CeresSolver + ceres_linear_solver: SPARSE_NORMAL_CHOLESKY + ceres_preconditioner: SCHUR_JACOBI + ceres_trust_strategy: LEVENBERG_MARQUARDT + ceres_dogleg_type: TRADITIONAL_DOGLEG + ceres_loss_function: None + + # ROS Parameters + odom_frame: odom + map_frame: map + base_frame: base_footprint + scan_topic: /scan + mode: mapping #localization + + # if you'd like to immediately start continuing a map at a given pose + # or at the dock, but they are mutually exclusive, if pose is given + # will use pose + #map_file_name: test_steve + #map_start_pose: [0.0, 0.0, 0.0] + #map_start_at_dock: true + + debug_logging: false + throttle_scans: 1 + transform_publish_period: 0.02 #if 0 never publishes odometry + map_update_interval: 1.0 + resolution: 0.05 + max_laser_range: 20.0 #for rastering images + minimum_time_interval: 0.5 + transform_timeout: 0.2 + tf_buffer_duration: 30. + stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps + enable_interactive_mode: true + + # General Parameters + use_scan_matching: true + use_scan_barycenter: true + minimum_travel_distance: 0.1 + minimum_travel_heading: 0.1 + scan_buffer_size: 10 + scan_buffer_maximum_scan_distance: 10.0 + link_match_minimum_response_fine: 0.1 + link_scan_maximum_distance: 1.5 + loop_search_maximum_distance: 3.0 + do_loop_closing: true + loop_match_minimum_chain_size: 10 + loop_match_maximum_variance_coarse: 3.0 + loop_match_minimum_response_coarse: 0.35 + loop_match_minimum_response_fine: 0.45 + + # Correlation Parameters - Correlation Parameters + correlation_search_space_dimension: 0.5 + correlation_search_space_resolution: 0.01 + correlation_search_space_smear_deviation: 0.1 + + # Correlation Parameters - Loop Closure Parameters + loop_search_space_dimension: 8.0 + loop_search_space_resolution: 0.05 + loop_search_space_smear_deviation: 0.03 + + # Scan Matcher Parameters + distance_variance_penalty: 0.5 + angle_variance_penalty: 1.0 + + fine_search_angle_offset: 0.00349 + coarse_search_angle_offset: 0.349 + coarse_angle_resolution: 0.0349 + minimum_angle_penalty: 0.9 + minimum_distance_penalty: 0.5 + use_response_expansion: true diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/nav2_params.yaml b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/nav2_params.yaml new file mode 100644 index 000000000..3eeaebed8 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/nav2_params.yaml @@ -0,0 +1,367 @@ +amcl: + ros__parameters: + use_sim_time: True + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "base_footprint" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "differential" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 1.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: scan + +amcl_map_client: + ros__parameters: + use_sim_time: True + +amcl_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + enable_groot_monitoring: True + groot_zmq_publisher_port: 1666 + groot_zmq_server_port: 1667 + default_nav_to_pose_bt_xml: RUNTIMEFILL + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_back_up_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + + +bt_navigator_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["goal_checker", "forward_goal_checker", "backward_goal_checker", "absolute_rotate_goal_checker", "undo_path_backwards_goal_checker"] + controller_plugins: ["FollowPath","BackwardLocalPlanner", "ForwardLocalPlanner", "PureSpinningLocalPlanner"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.001 + movement_time_allowance: 3000.0 + + # Goal checker parameters + goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.257 + yaw_goal_tolerance: 0.165 # almost free orientation + stateful: True + backward_goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.09 + yaw_goal_tolerance: 0.05 + stateful: True + forward_goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.09 + yaw_goal_tolerance: 0.05 + stateful: True + absolute_rotate_goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.15 + yaw_goal_tolerance: 0.01 #0.005 + stateful: True + undo_path_backwards_goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.03 # 4 cm + yaw_goal_tolerance: 0.1 + stateful: True + + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.3 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.3 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.157 + yaw_goal_tolerance: 0.15 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + BackwardLocalPlanner: + plugin: "backward_local_planner::BackwardLocalPlanner" + transform_tolerance: 0.5 + k_rho: -1.4 # linear distance to the goal gain, default = -1.0 (related with carrot distance) + k_alpha: -0.1 #-0.1 + k_betta: -0.1 # -1.0 + carrot_distance: 0.3 #meters default 0.5 + carrot_angular_distance: 0.3 # no constraint, max 3.1416 + pure_spinning_straight_line_mode: true + max_linear_x_speed: 0.25 + ForwardLocalPlanner: + plugin: "cl_nav2z::forward_local_planner::ForwardLocalPlanner" + transform_tolerance: 0.5 + k_rho: 2.5 + k_alpha: -0.4 + k_betta: -1.0 + carrot_distance: 0.2 #meters + carrot_angular_distance: 0.5 # no constraint, max 3.1416 + max_linear_x_speed: 0.3 + max_angular_z_speed: 0.2 + PureSpinningLocalPlanner: + plugin: "pure_spinning_local_planner::PureSpinningLocalPlanner" + transform_tolerance: 0.5 + k_betta: 10.0 + max_angular_z_speed: 0.2 + use_shortest_angular_distance: true + + +controller_server_rclcpp_node: + ros__parameters: + use_sim_time: True + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + use_sim_time: True + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 15.0 + raytrace_min_range: 0.0 + obstacle_max_range: 15.5 + obstacle_min_range: 0.0 + static_layer: + map_subscribe_transient_local: True + always_send_full_costmap: True + local_costmap_client: + ros__parameters: + use_sim_time: True + local_costmap_rclcpp_node: + ros__parameters: + use_sim_time: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + transform_tolerance: 5.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 15.0 + raytrace_min_range: 0.0 + obstacle_max_range: 15.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + global_costmap_client: + ros__parameters: + use_sim_time: True + global_costmap_rclcpp_node: + ros__parameters: + use_sim_time: True + +map_server: + ros__parameters: + use_sim_time: True + yaml_filename: "turtlebot3_world.yaml" + +map_saver: + ros__parameters: + use_sim_time: True + save_map_timeout: 5000 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased", "ForwardGlobalPlanner", "BackwardGlobalPlanner", "UndoPathGlobalPlanner"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 10.0 #meters + use_astar: false + allow_unknown: true + ForwardGlobalPlanner: + plugin: "forward_global_planner::ForwardGlobalPlanner" + transform_tolerance: 0.5 + BackwardGlobalPlanner: + plugin: "backward_global_planner::BackwardGlobalPlanner" + transform_tolerance: 0.5 + UndoPathGlobalPlanner: + plugin: "undo_path_global_planner::UndoPathGlobalPlanner" + transform_tolerance: 0.5 + +planner_server_rclcpp_node: + ros__parameters: + use_sim_time: True + +recoveries_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + recovery_plugins: ["spin", "backup", "wait"] + #recovery_plugins: ["spin"] + spin: + plugin: "nav2_recoveries/Spin" + backup: + plugin: "nav2_recoveries/BackUp" + wait: + plugin: "nav2_recoveries/Wait" + global_frame: odom + robot_base_frame: base_link + transform_timeout: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/navigation_tree.xml b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/navigation_tree.xml new file mode 100644 index 000000000..ebd9f201f --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/navigation_tree.xml @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan.yaml b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan.yaml new file mode 100644 index 000000000..64adbe1b0 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan.yaml @@ -0,0 +1,82 @@ +waypoints: + # POINT 0 + - position: + x: 3.0 + y: -2.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 1 + - position: + x: -5.75 + y: 3.5 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 2 + - position: + x: -10.25 + y: -4.25 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 3 + - position: + x: 2.0 + y: -8.58 # north west corner of the room + #y: -12.35 # south west corner of the room + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 4 + - position: + x: -12.00 + y: 12.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 5 + - position: + x: -10.0 + y: 14.5 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 6 + - position: + x: -0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + #POINT 7 + - position: + x: 6.0 + y: 8.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_husky/nav2_params.yaml b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_husky/nav2_params.yaml new file mode 100644 index 000000000..08e344730 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_husky/nav2_params.yaml @@ -0,0 +1,367 @@ +amcl: + ros__parameters: + use_sim_time: True + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "base_footprint" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "differential" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 1.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: scan + +amcl_map_client: + ros__parameters: + use_sim_time: True + +amcl_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + enable_groot_monitoring: True + groot_zmq_publisher_port: 1666 + groot_zmq_server_port: 1667 + default_nav_to_pose_bt_xml: RUNTIMEFILL + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_back_up_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + + +bt_navigator_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["goal_checker", "forward_goal_checker", "backward_goal_checker", "absolute_rotate_goal_checker", "undo_path_backwards_goal_checker"] + controller_plugins: ["FollowPath","BackwardLocalPlanner", "ForwardLocalPlanner", "PureSpinningLocalPlanner"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.001 + movement_time_allowance: 3000.0 + + # Goal checker parameters + goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.257 + yaw_goal_tolerance: 0.165 # almost free orientation + stateful: True + backward_goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.09 + yaw_goal_tolerance: 0.05 + stateful: True + forward_goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.09 + yaw_goal_tolerance: 0.05 + stateful: True + absolute_rotate_goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.15 + yaw_goal_tolerance: 0.01 #0.005 + stateful: True + undo_path_backwards_goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.03 # 4 cm + yaw_goal_tolerance: 0.1 + stateful: True + + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.3 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.05 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.157 + yaw_goal_tolerance: 0.15 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + BackwardLocalPlanner: + plugin: "backward_local_planner::BackwardLocalPlanner" + transform_tolerance: 0.5 + k_rho: -1.4 # linear distance to the goal gain, default = -1.0 (related with carrot distance) + k_alpha: -0.1 #-0.1 + k_betta: -0.1 # -1.0 + carrot_distance: 0.3 #meters default 0.5 + carrot_angular_distance: 0.3 # no constraint, max 3.1416 + pure_spinning_straight_line_mode: true + max_linear_x_speed: 0.25 + ForwardLocalPlanner: + plugin: "cl_nav2z::forward_local_planner::ForwardLocalPlanner" + transform_tolerance: 0.5 + k_rho: 2.5 + k_alpha: -0.4 + k_betta: -1.0 + carrot_distance: 0.2 #meters + carrot_angular_distance: 0.5 # no constraint, max 3.1416 + max_linear_x_speed: 0.3 + max_angular_z_speed: 0.2 + PureSpinningLocalPlanner: + plugin: "pure_spinning_local_planner::PureSpinningLocalPlanner" + transform_tolerance: 0.5 + k_betta: 10.0 + max_angular_z_speed: 0.2 + use_shortest_angular_distance: true + + +controller_server_rclcpp_node: + ros__parameters: + use_sim_time: True + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + use_sim_time: True + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 15.0 + raytrace_min_range: 0.0 + obstacle_max_range: 15.5 + obstacle_min_range: 0.0 + static_layer: + map_subscribe_transient_local: True + always_send_full_costmap: True + local_costmap_client: + ros__parameters: + use_sim_time: True + local_costmap_rclcpp_node: + ros__parameters: + use_sim_time: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + transform_tolerance: 5.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 15.0 + raytrace_min_range: 0.0 + obstacle_max_range: 15.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + global_costmap_client: + ros__parameters: + use_sim_time: True + global_costmap_rclcpp_node: + ros__parameters: + use_sim_time: True + +map_server: + ros__parameters: + use_sim_time: True + yaml_filename: "turtlebot3_world.yaml" + +map_saver: + ros__parameters: + use_sim_time: True + save_map_timeout: 5000 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased", "ForwardGlobalPlanner", "BackwardGlobalPlanner", "UndoPathGlobalPlanner"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 10.0 #meters + use_astar: false + allow_unknown: true + ForwardGlobalPlanner: + plugin: "forward_global_planner::ForwardGlobalPlanner" + transform_tolerance: 0.5 + BackwardGlobalPlanner: + plugin: "backward_global_planner::BackwardGlobalPlanner" + transform_tolerance: 0.5 + UndoPathGlobalPlanner: + plugin: "undo_path_global_planner::UndoPathGlobalPlanner" + transform_tolerance: 0.5 + +planner_server_rclcpp_node: + ros__parameters: + use_sim_time: True + +recoveries_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + recovery_plugins: ["spin", "backup", "wait"] + #recovery_plugins: ["spin"] + spin: + plugin: "nav2_recoveries/Spin" + backup: + plugin: "nav2_recoveries/BackUp" + wait: + plugin: "nav2_recoveries/Wait" + global_frame: odom + robot_base_frame: base_link + transform_timeout: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_husky/navigation_tree.xml b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_husky/navigation_tree.xml new file mode 100644 index 000000000..e599e5d4f --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_husky/navigation_tree.xml @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_husky/waypoints_plan.yaml b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_husky/waypoints_plan.yaml new file mode 100644 index 000000000..64adbe1b0 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_husky/waypoints_plan.yaml @@ -0,0 +1,82 @@ +waypoints: + # POINT 0 + - position: + x: 3.0 + y: -2.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 1 + - position: + x: -5.75 + y: 3.5 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 2 + - position: + x: -10.25 + y: -4.25 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 3 + - position: + x: 2.0 + y: -8.58 # north west corner of the room + #y: -12.35 # south west corner of the room + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 4 + - position: + x: -12.00 + y: 12.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 5 + - position: + x: -10.0 + y: 14.5 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 6 + - position: + x: -0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + #POINT 7 + - position: + x: 6.0 + y: 8.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_turtlebot/nav2_params.yaml b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_turtlebot/nav2_params.yaml new file mode 100644 index 000000000..3eeaebed8 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_turtlebot/nav2_params.yaml @@ -0,0 +1,367 @@ +amcl: + ros__parameters: + use_sim_time: True + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "base_footprint" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "differential" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 1.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: scan + +amcl_map_client: + ros__parameters: + use_sim_time: True + +amcl_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + enable_groot_monitoring: True + groot_zmq_publisher_port: 1666 + groot_zmq_server_port: 1667 + default_nav_to_pose_bt_xml: RUNTIMEFILL + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_back_up_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + + +bt_navigator_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["goal_checker", "forward_goal_checker", "backward_goal_checker", "absolute_rotate_goal_checker", "undo_path_backwards_goal_checker"] + controller_plugins: ["FollowPath","BackwardLocalPlanner", "ForwardLocalPlanner", "PureSpinningLocalPlanner"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.001 + movement_time_allowance: 3000.0 + + # Goal checker parameters + goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.257 + yaw_goal_tolerance: 0.165 # almost free orientation + stateful: True + backward_goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.09 + yaw_goal_tolerance: 0.05 + stateful: True + forward_goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.09 + yaw_goal_tolerance: 0.05 + stateful: True + absolute_rotate_goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.15 + yaw_goal_tolerance: 0.01 #0.005 + stateful: True + undo_path_backwards_goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.03 # 4 cm + yaw_goal_tolerance: 0.1 + stateful: True + + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.3 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.3 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.157 + yaw_goal_tolerance: 0.15 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + BackwardLocalPlanner: + plugin: "backward_local_planner::BackwardLocalPlanner" + transform_tolerance: 0.5 + k_rho: -1.4 # linear distance to the goal gain, default = -1.0 (related with carrot distance) + k_alpha: -0.1 #-0.1 + k_betta: -0.1 # -1.0 + carrot_distance: 0.3 #meters default 0.5 + carrot_angular_distance: 0.3 # no constraint, max 3.1416 + pure_spinning_straight_line_mode: true + max_linear_x_speed: 0.25 + ForwardLocalPlanner: + plugin: "cl_nav2z::forward_local_planner::ForwardLocalPlanner" + transform_tolerance: 0.5 + k_rho: 2.5 + k_alpha: -0.4 + k_betta: -1.0 + carrot_distance: 0.2 #meters + carrot_angular_distance: 0.5 # no constraint, max 3.1416 + max_linear_x_speed: 0.3 + max_angular_z_speed: 0.2 + PureSpinningLocalPlanner: + plugin: "pure_spinning_local_planner::PureSpinningLocalPlanner" + transform_tolerance: 0.5 + k_betta: 10.0 + max_angular_z_speed: 0.2 + use_shortest_angular_distance: true + + +controller_server_rclcpp_node: + ros__parameters: + use_sim_time: True + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + use_sim_time: True + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 15.0 + raytrace_min_range: 0.0 + obstacle_max_range: 15.5 + obstacle_min_range: 0.0 + static_layer: + map_subscribe_transient_local: True + always_send_full_costmap: True + local_costmap_client: + ros__parameters: + use_sim_time: True + local_costmap_rclcpp_node: + ros__parameters: + use_sim_time: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + transform_tolerance: 5.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 15.0 + raytrace_min_range: 0.0 + obstacle_max_range: 15.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + global_costmap_client: + ros__parameters: + use_sim_time: True + global_costmap_rclcpp_node: + ros__parameters: + use_sim_time: True + +map_server: + ros__parameters: + use_sim_time: True + yaml_filename: "turtlebot3_world.yaml" + +map_saver: + ros__parameters: + use_sim_time: True + save_map_timeout: 5000 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased", "ForwardGlobalPlanner", "BackwardGlobalPlanner", "UndoPathGlobalPlanner"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 10.0 #meters + use_astar: false + allow_unknown: true + ForwardGlobalPlanner: + plugin: "forward_global_planner::ForwardGlobalPlanner" + transform_tolerance: 0.5 + BackwardGlobalPlanner: + plugin: "backward_global_planner::BackwardGlobalPlanner" + transform_tolerance: 0.5 + UndoPathGlobalPlanner: + plugin: "undo_path_global_planner::UndoPathGlobalPlanner" + transform_tolerance: 0.5 + +planner_server_rclcpp_node: + ros__parameters: + use_sim_time: True + +recoveries_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + recovery_plugins: ["spin", "backup", "wait"] + #recovery_plugins: ["spin"] + spin: + plugin: "nav2_recoveries/Spin" + backup: + plugin: "nav2_recoveries/BackUp" + wait: + plugin: "nav2_recoveries/Wait" + global_frame: odom + robot_base_frame: base_link + transform_timeout: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_turtlebot/navigation_tree.xml b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_turtlebot/navigation_tree.xml new file mode 100644 index 000000000..e599e5d4f --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_turtlebot/navigation_tree.xml @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_turtlebot/waypoints_plan.yaml b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_turtlebot/waypoints_plan.yaml new file mode 100644 index 000000000..64adbe1b0 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_turtlebot/waypoints_plan.yaml @@ -0,0 +1,82 @@ +waypoints: + # POINT 0 + - position: + x: 3.0 + y: -2.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 1 + - position: + x: -5.75 + y: 3.5 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 2 + - position: + x: -10.25 + y: -4.25 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 3 + - position: + x: 2.0 + y: -8.58 # north west corner of the room + #y: -12.35 # south west corner of the room + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 4 + - position: + x: -12.00 + y: 12.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 5 + - position: + x: -10.0 + y: 14.5 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 6 + - position: + x: -0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + #POINT 7 + - position: + x: 6.0 + y: 8.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/params/sm_dance_bot_config.yaml b/smacc2_sm_reference_library/sm_dancebot_ue/params/sm_dance_bot_config.yaml new file mode 100644 index 000000000..977ed0301 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/params/sm_dance_bot_config.yaml @@ -0,0 +1,9 @@ +SmDanceBot: + ros__parameters: + use_sim_time: true + signal_detector_loop_freq: 20.0 + clear_angular_distance_threshold: 0.1 # 0.05 + clear_point_distance_threshold: 0.4 #0.1 + record_angular_distance_threshold: 0.005 + record_point_distance_threshold: 0.1 + waypoints_plan: $(pkg_share)/params/nav2z_client/waypoints_plan.yaml diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/rviz/nav2_default_view.rviz b/smacc2_sm_reference_library/sm_dancebot_ue/rviz/nav2_default_view.rviz new file mode 100644 index 000000000..6a506a1ab --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/rviz/nav2_default_view.rviz @@ -0,0 +1,811 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /TF1 + - /TF1/Frames1 + - /TF1/Tree1 + - /LaserScan1 + - /BackwardLocalPlan1 + Splitter Ratio: 0.5833333134651184 + Tree Height: 865 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: nav2_rviz_plugins/Navigation 2 + Name: Navigation 2 +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 + - 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: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: "" + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: false + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + base_footprint: + Value: true + base_link: + Value: true + base_scan: + Value: true + camera_depth_frame: + Value: true + camera_depth_optical_frame: + Value: true + camera_link: + Value: true + camera_rgb_frame: + Value: true + camera_rgb_optical_frame: + Value: true + caster_back_left_link: + Value: true + caster_back_right_link: + Value: true + imu_link: + Value: true + map: + Value: true + odom: + Value: true + wheel_left_link: + Value: true + wheel_right_link: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: false + Show Axes: true + Show Names: false + Tree: + map: + odom: + base_footprint: + base_link: + base_scan: + {} + camera_link: + camera_depth_frame: + camera_depth_optical_frame: + {} + camera_rgb_frame: + camera_rgb_optical_frame: + {} + caster_back_left_link: + {} + caster_back_right_link: + {} + imu_link: + {} + wheel_left_link: + {} + wheel_right_link: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 255; 0; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 5 + Size (m): 4 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: "" + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Bumper Hit + Position Transformer: "" + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Spheres + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /mobile_base/sensors/bumper_pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: true + Enabled: true + Name: Map + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Arrow Length: 0.019999999552965164 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz_default_plugins/PoseArray + Color: 0; 180; 0 + Enabled: true + Head Length: 0.07000000029802322 + Head Radius: 0.029999999329447746 + Name: Amcl Particle Swarm + Shaft Length: 0.23000000417232513 + Shaft Radius: 0.009999999776482582 + Shape: Arrow (Flat) + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /particlecloud + Value: true + - Class: rviz_common/Group + Displays: + - Alpha: 0.30000001192092896 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Global Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 0; 0 + Enabled: true + Head Diameter: 0.019999999552965164 + Head Length: 0.019999999552965164 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: Arrows + Radius: 0.029999999329447746 + Shaft Diameter: 0.004999999888241291 + Shaft Length: 0.019999999552965164 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /plan + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud + Color: 125; 125; 125 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: VoxelGrid + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Boxes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/voxel_marked_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: false + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/published_footprint + Value: false + Enabled: true + Name: Global Planner + - Class: rviz_common/Group + Displays: + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Local Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 0; 12; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Local Plan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_plan + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Trajectories + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /marker + Value: false + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: true + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/published_footprint + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: VoxelGrid + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/voxel_marked_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Controller + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: RealsenseCamera + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /intel_realsense_r200_depth/image_raw + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: RealsenseDepthImage + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /intel_realsense_r200_depth/points + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: false + Name: Realsense + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.10000000149011612 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: CpOdomTracker + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: Axes + Radius: 0.009999999776482582 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /odom_tracker_path + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: BackwardsGlobalPlan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /backward_planner/global_plan + Value: true + - Angle Tolerance: 0.10000000149011612 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: false + Enabled: true + Keep: 10000 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0 + Head Radius: 0 + Shaft Length: 0.02800000086426735 + Shaft Radius: 0.009999999776482582 + Value: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /odom + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: WayPointsMarkers + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /waypoints + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VisualizationMarkers + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: visualization_marker_array + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: BackwardsGlobalPlanMarkers + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: backward_planner/markers + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: BackwardLocalPlannerGoalMarker + Namespaces: + my_namespace2: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /backward_local_planner/goal_marker + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: ForwardCarrot + Namespaces: + my_namespace2: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /forward_local_planner/carrot_goal_marker + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: UndoGlobalPlannerMarkers + Namespaces: + my_namespace2: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /undo_path_planner/markers + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: UndoGlobalPlan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /undo_path_planner/global_plan + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 255; 0 + Enabled: true + Head Diameter: 0.05999999865889549 + Head Length: 0.03999999910593033 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.0010000000474974513 + Name: BackwardLocalPlan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 0 + Pose Style: Arrows + Radius: 0.029999999329447746 + Shaft Diameter: 0.019999999552965164 + Shaft Length: 0.009999999776482582 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /backward_local_planner/path + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - 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/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + - Class: nav2_rviz_plugins/GoalTool + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Angle: -4.785000801086426 + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Scale: -123.52355194091797 + Target Frame: base_link + Value: TopDownOrtho (rviz_default_plugins) + X: -0.6155462265014648 + Y: -0.8152815103530884 + Saved: ~ +Window Geometry: + Displays: + collapsed: true + Height: 1016 + Hide Left Dock: true + Hide Right Dock: true + Navigation 2: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000002860000039efc020000000afb0000001200530065006c0065006300740069006f006e000000003d000000830000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003200000002da000001010000013900fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000002800ffffff000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000007380000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + RealsenseCamera: + collapsed: false + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1848 + X: 72 + Y: 27 diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/rviz/nav2_namespaced_view.rviz b/smacc2_sm_reference_library/sm_dancebot_ue/rviz/nav2_namespaced_view.rviz new file mode 100644 index 000000000..2a024f75b --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/rviz/nav2_namespaced_view.rviz @@ -0,0 +1,371 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 195 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /RobotModel1/Status1 + - /TF1/Frames1 + - /TF1/Tree1 + - /Global Planner1/Global Costmap1 + Splitter Ratio: 0.5833333134651184 + Tree Height: 464 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: nav2_rviz_plugins/Navigation 2 + Name: Navigation 2 +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 + - 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: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: "" + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Bumper Hit + Position Transformer: "" + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Spheres + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /mobile_base/sensors/bumper_pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: true + Enabled: true + Name: Map + Topic: + Depth: 1 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map + Use Timestamp: false + Value: true + - Alpha: 1 + Arrow Length: 0.019999999552965164 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz_default_plugins/PoseArray + Color: 0; 180; 0 + Enabled: true + Head Length: 0.07000000029802322 + Head Radius: 0.029999999329447746 + Name: Amcl Particle Swarm + Shaft Length: 0.23000000417232513 + Shaft Radius: 0.009999999776482582 + Shape: Arrow (Flat) + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /particlecloud + Value: true + - Class: rviz_common/Group + Displays: + - Alpha: 0.30000001192092896 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Global Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 0; 0 + Enabled: true + Head Diameter: 0.019999999552965164 + Head Length: 0.019999999552965164 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: Arrows + Radius: 0.029999999329447746 + Shaft Diameter: 0.004999999888241291 + Shaft Length: 0.019999999552965164 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /plan + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: false + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/published_footprint + Value: false + Enabled: true + Name: Global Planner + - Class: rviz_common/Group + Displays: + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Local Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 0; 12; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Local Plan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_plan + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Trajectories + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /marker + Value: false + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: true + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/published_footprint + Value: true + Enabled: true + Name: Controller + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - 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 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: nav2_rviz_plugins/GoalTool + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Angle: -1.5700000524520874 + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Scale: 134.638427734375 + Target Frame: + Value: TopDownOrtho (rviz_default_plugins) + X: -0.032615214586257935 + Y: -0.0801941454410553 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 914 + Hide Left Dock: false + Hide Right Dock: true + Navigation 2: + collapsed: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a00000338fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002c4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003201000003070000006e0000006200ffffff000000010000010f00000338fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000338000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004990000033800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1545 + X: 186 + Y: 117 diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/servers/led_action_server/action/LEDControl.action b/smacc2_sm_reference_library/sm_dancebot_ue/servers/led_action_server/action/LEDControl.action new file mode 100644 index 000000000..e4dc1d976 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/servers/led_action_server/action/LEDControl.action @@ -0,0 +1,18 @@ +uint8 command + +# Possible command values +uint8 CMD_ON = 0 +uint8 CMD_OFF = 1 +--- +uint8 state + +# Possible feeedback values (tool states) +uint8 STATE_UNKNOWN = 0 +uint8 STATE_RUNNING = 1 +uint8 STATE_IDLE = 2 +--- +uint8 result + +# Possible result values +uint8 RESULT_SUCCESS = 0 +uint8 RESULT_ERROR = 1 diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/servers/led_action_server/src/led_action_server_node.cpp b/smacc2_sm_reference_library/sm_dancebot_ue/servers/led_action_server/src/led_action_server_node.cpp new file mode 100644 index 000000000..c72c64e90 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/servers/led_action_server/src/led_action_server_node.cpp @@ -0,0 +1,230 @@ +// 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 + * + ******************************************************************************************************************/ +// #include +// #include +// #include +// #include + +#include +#include +#include +#include +#include +// #include +// #include + +#include +#include +#include + +// This class describes a preemptable-on/off tool action server to be used from smacc +// shows in rviz a sphere whose color describe the current state (unknown, running, idle) +class LEDActionServer : public rclcpp::Node +{ +public: + std::shared_ptr> as_; + using GoalHandleLEDControl = + rclcpp_action::ServerGoalHandle; + + rclcpp::Publisher::SharedPtr stateMarkerPublisher_; + + uint8_t cmd; + + uint8_t currentState_; + + /** +****************************************************************************************************************** +* constructor() +****************************************************************************************************************** +*/ + LEDActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) + : Node("led_action_server_node", options) + { + currentState_ = sm_dancebot_ue::action::LEDControl::Result::STATE_UNKNOWN; + } + + /** +****************************************************************************************************************** +* execute() +****************************************************************************************************************** +*/ + void execute(const std::shared_ptr + gh) // Note: "Action" is not appended to DoDishes here + { + auto goal = gh->get_goal(); + RCLCPP_INFO_STREAM(get_logger(), "Tool action server request: " << goal->command); + + cmd = goal->command; + + if (goal->command == sm_dancebot_ue::action::LEDControl_Goal::CMD_ON) + { + RCLCPP_INFO(this->get_logger(), "ON"); + currentState_ = sm_dancebot_ue::action::LEDControl_Result::STATE_RUNNING; + } + else if (goal->command == sm_dancebot_ue::action::LEDControl_Goal::CMD_OFF) + { + RCLCPP_INFO(this->get_logger(), "OFF"); + currentState_ = sm_dancebot_ue::action::LEDControl_Result::STATE_IDLE; + } + + auto feedback_msg = std::make_shared(); + + // 10Hz internal loop + rclcpp::Rate rate(20); + + while (rclcpp::ok()) + { + gh->publish_feedback(feedback_msg); + + publishStateMarker(); + rate.sleep(); + RCLCPP_INFO_THROTTLE(this->get_logger(), *(this->get_clock()), 2000, "Loop feedback"); + } + + auto result = std::make_shared(); + result->state = this->currentState_; + + // never reach succeeded because were are interested in keeping the feedback alive + //as_->setSucceeded(); + gh->succeed(result); + } + + rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID & /*uuid*/, + std::shared_ptr /*goal*/) + { + // (void)uuid; + // // Let's reject sequences that are over 9000 + // if (goal->order > 9000) { + // return rclcpp_action::GoalResponse::REJECT; + // } + + // lets accept everything + RCLCPP_INFO(this->get_logger(), "Handle goal"); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } + + rclcpp_action::CancelResponse handle_cancel( + const std::shared_ptr /*goal_handle*/) + { + RCLCPP_INFO(rclcpp::get_logger("server"), "Got request to cancel goal"); + //(void)goal_handle; + return rclcpp_action::CancelResponse::ACCEPT; + } + + void handle_accepted(const std::shared_ptr goal_handle) + { + // this needs to return quickly to avoid blocking the executor, so spin up a new thread + std::thread{std::bind(&LEDActionServer::execute, this, std::placeholders::_1), goal_handle} + .detach(); + } + + /** +****************************************************************************************************************** +* run() +****************************************************************************************************************** +*/ + void run() + { + RCLCPP_INFO(this->get_logger(), "Creating tool action server"); + //as_ = std::make_shared(n, "led_action_server", boost::bind(&LEDActionServer::execute, this, _1), false); + + this->as_ = rclcpp_action::create_server( + this, "led_action_server", + std::bind(&LEDActionServer::handle_goal, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&LEDActionServer::handle_cancel, this, std::placeholders::_1), + std::bind(&LEDActionServer::handle_accepted, this, std::placeholders::_1)); + + RCLCPP_INFO(get_logger(), "Starting Tool Action Server"); + stateMarkerPublisher_ = + this->create_publisher("tool_markers", 1); + } + + /** +****************************************************************************************************************** +* publishStateMarker() +****************************************************************************************************************** +*/ + void publishStateMarker() + { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "base_link"; + marker.header.stamp = this->now(); + + marker.ns = "tool_namespace"; + marker.id = 0; + marker.type = visualization_msgs::msg::Marker::SPHERE; + marker.action = visualization_msgs::msg::Marker::ADD; + + marker.scale.x = 0.2; + marker.scale.y = 0.2; + marker.scale.z = 0.2; + + marker.color.a = 1; + + if (currentState_ == sm_dancebot_ue::action::LEDControl::Result::STATE_RUNNING) + { + // show green ball + marker.color.r = 0; + marker.color.g = 1; + marker.color.b = 0; + } + else if (currentState_ == sm_dancebot_ue::action::LEDControl::Result::STATE_IDLE) + { + // show gray ball + marker.color.r = 0.7; + marker.color.g = 0.7; + marker.color.b = 0.7; + } + else + { + // show black ball + marker.color.r = 0; + marker.color.g = 0; + marker.color.b = 0; + } + + marker.pose.orientation.w = 1; + marker.pose.position.x = 0; + marker.pose.position.y = 0; + marker.pose.position.z = 1; + + visualization_msgs::msg::MarkerArray ma; + ma.markers.push_back(marker); + + stateMarkerPublisher_->publish(ma); + } +}; + +/** +****************************************************************************************************************** +* main() +****************************************************************************************************************** +*/ +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto ledactionserver = std::make_shared(); + ledactionserver->run(); + + rclcpp::spin(ledactionserver); + rclcpp::shutdown(); + return 0; +} diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/servers/service_node_3/service_node_3.py b/smacc2_sm_reference_library/sm_dancebot_ue/servers/service_node_3/service_node_3.py new file mode 100644 index 000000000..a6694cde3 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/servers/service_node_3/service_node_3.py @@ -0,0 +1,47 @@ +#!/usr/bin/env python3 + +# 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. + +# import roslib +# import rospy +import rclpy +from rclpy.node import Node + +import std_srvs +import std_srvs.srv + +if __name__ == "__main__": + + class Service3(Node): + def __init__(self): + super().__init__("Service3") + self.s = self.create_service( + std_srvs.srv.SetBool, "service_node3", self.set_bool_service + ) + + def set_bool_service(self, req, res): + self.get_logger().info("RECEIVING SET BOOL SERVICE REQUEST: value=" + str(req.data)) + + res.message = "OK, value set" + res.success = True + return res + + rclpy.init(args=None) + s = Service3() + + # s = rospy.Service('service_node3', std_srvs.srv.SetBool, set_bool_service) + # rospy.spin() + rclpy.spin(s) + rclpy.shutdown() diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/servers/temperature_sensor_node/src/temperature_sensor_node.cpp b/smacc2_sm_reference_library/sm_dancebot_ue/servers/temperature_sensor_node/src/temperature_sensor_node.cpp new file mode 100644 index 000000000..ce66d640a --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/servers/temperature_sensor_node/src/temperature_sensor_node.cpp @@ -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. + +#include +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + + auto nh = rclcpp::Node::make_shared("temperature_sensor_node"); + auto pub = nh->create_publisher("/temperature", 1); + + rclcpp::Rate r(10); + + while (rclcpp::ok()) + { + sensor_msgs::msg::Temperature msg; + msg.temperature = sin(nh->now().seconds() / 2.0) * 50; + pub->publish(msg); + + r.sleep(); + rclcpp::spin_some(nh); + } +} diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/clients/cl_led/cl_led.cpp b/smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/clients/cl_led/cl_led.cpp new file mode 100644 index 000000000..9725f82eb --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/clients/cl_led/cl_led.cpp @@ -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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#include +//#include + +namespace sm_dancebot_ue +{ +namespace cl_led +{ +ClLED::ClLED(std::string actionServerName) +: SmaccActionClientBase(actionServerName) +{ +} + +std::string ClLED::getName() const { return "TOOL ACTION CLIENT"; } + +ClLED::~ClLED() {} + +std::ostream & operator<<( + std::ostream & out, const sm_dancebot_ue::action::LEDControl::Goal & msg) +{ + out << "LED CONTROL: " << msg.command; + return out; +} + +} // namespace cl_led + +//PLUGINLIB_EXPORT_CLASS(cl_led::ClLED, smacc2::ISmaccComponent) +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/sm_dancebot_ue.cpp b/smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/sm_dancebot_ue.cpp new file mode 100644 index 000000000..7794ed0ce --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/sm_dancebot_ue.cpp @@ -0,0 +1,27 @@ +// 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 + * + ******************************************************************************************************************/ + +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + smacc2::run(); +} diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/urdf/turtlebot3_waffle.urdf b/smacc2_sm_reference_library/sm_dancebot_ue/urdf/turtlebot3_waffle.urdf new file mode 100644 index 000000000..c92483449 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/urdf/turtlebot3_waffle.urdf @@ -0,0 +1,293 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/worlds/ridgeback_race.world b/smacc2_sm_reference_library/sm_dancebot_ue/worlds/ridgeback_race.world new file mode 100644 index 000000000..3ff47663c --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/worlds/ridgeback_race.world @@ -0,0 +1,4864 @@ + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + 0 + 0 + 0 + + + + 1 + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + + + + + + + + 10 + + + 0 + + + 0 0 1 + 100 100 + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + + + 0.001 + 1 + 1000 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + EARTH_WGS84 + 0 + 0 + 0 + 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -1 -6 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -4 1.55828 0 0 -0 0 + + + 3116 755000000 + 356 954966630 + 1605650970 323926707 + 355029 + + -2.6481 -4.01574 -0.045465 0 0 -0.74752 + 1 1 1 + + 8.54538 -6.87025 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.25439 -7.04543 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 1.49008 -7.104 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.8196 -6.78892 -0.045465 0 0 -0.74752 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.6481 -4.01574 -0.045465 0 0 -2.31822 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.78254 -6.92811 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 5.0182 -6.98712 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.8196 -6.78892 0.820535 0 0 -0.74752 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -0.070844 -6.42684 -0.045465 0 0 -2.31822 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1.3598 -5.22163 -0.045465 0 0 -2.31822 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -15.3977 -1.55916 0 0 -0 1.04571 + 1 1 1 + + -15.0829 9.98827 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.7448 4.86628 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.2984 3.15835 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -15.4433 11.2132 0 0 -0 1.04571 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -15.3977 -1.55916 0 0 0 -0.524988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.6375 8.28162 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.1908 6.57377 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -15.4433 11.2132 0.866 0 -0 1.04571 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.6146 1.4865 0 0 0 -0.524988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.5057 -0.036576 0 0 0 -0.524988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -2.60683 -10.2645 0 0 0 -0.76918 + 1 1 1 + + 8.5222 -13.3608 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.22866 -13.4213 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 1.46349 -13.4417 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.79789 -13.3071 0 0 0 -0.76918 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.60683 -10.2645 0 0 0 -2.33988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.75852 -13.3804 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 4.99332 -13.4012 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.79789 -13.3071 0.866 0 0 -0.76918 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -0.082399 -12.7309 0 0 0 -2.33988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1.34495 -11.498 0 0 0 -2.33988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3347 -5.65314 0 0 -0 1.56441 + 1 1 1 + + 4.88352 4.5315 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 8.58481 0.746571 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.81915 -0.515431 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.96327 5.4166 0 0 -0 1.56441 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.3347 -5.65314 0 0 0 -0.006293 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.11634 3.2701 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 7.35101 2.00838 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.96327 5.4166 0.866 0 -0 1.56441 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.3734 -2.1241 0 0 0 -0.006293 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.3545 -3.88863 0 0 0 -0.006293 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -16.0335 -6.41848 0 0 0 -0.7879 + 1 1 1 + + -4.96438 -9.72253 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -10.2581 -9.68398 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -12.0234 -9.67127 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -3.68791 -9.69271 0 0 0 -0.7879 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -16.0335 -6.41848 0 0 0 -2.3586 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -6.72812 -9.70918 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -8.4934 -9.69692 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -3.68791 -9.69271 0.866 0 0 -0.7879 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.5557 -8.93166 0 0 0 -2.3586 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.7949 -7.6754 0 0 0 -2.3586 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -1 16 0 0 -0 0 + 1 1 1 + + 9.2248 10.3902 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 5.49818 14.15 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 4.2559 15.4033 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.0288 9.55797 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1 16 0 0 0 -1.5707 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 7.98256 11.6439 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.74031 12.8974 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.0288 9.55797 0.866 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 2.52922 15.9838 0 0 0 -1.5707 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 0.7646 15.9914 0 0 0 -1.5707 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -14.6681 12.3809 0 0 -0 0.810038 + 1 1 1 + + -3.55516 15.9191 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -8.84779 15.8121 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -10.6121 15.7764 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.39802 15.9276 0 0 -0 0.810038 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.6681 12.3809 0 0 0 -0.760662 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -5.3197 15.8837 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -7.08412 15.8482 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.39802 15.9276 0.866 0 -0 0.810038 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -12.2231 14.926 0 0 0 -0.760662 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.4453 13.6531 0 0 0 -0.760662 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -8.62781 9.94618 0 0 -0 0.2056 + 1 1 1 + + 2.5269 6.54195 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1.88879 9.46171 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -3.36079 10.4351 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.48387 5.89136 0 0 -0 0.205596 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -8.62781 9.94618 0 0 0 -1.3651 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 1.05488 7.51562 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -0.41711 8.4891 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.48387 5.89136 0.866 0 -0 0.205596 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -5.16961 10.6508 0 0 0 -1.3651 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -6.89862 10.298 0 0 0 -1.3651 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -16.7548 -4.64388 0 0 -0 0 + 1 1 1 + + -16.7548 -4.64388 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -17.1282 -4.13814 0 0 -0 0 + 1 1 1 + + -17.1282 -4.13814 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3218 -9.21435 0 0 -0 0 + 1 1 1 + + 10.3218 -9.21435 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3067 -12.7762 0 0 -0 0 + 1 1 1 + + 10.3067 -12.7762 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3176 -7.4365 0 0 -0 0 + 1 1 1 + + 10.3176 -7.4365 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3136 -11.0025 0 0 -0 0 + 1 1 1 + + 10.3136 -11.0025 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -16.2875 -3.11229 0 0 0 -0.509254 + 1 1 1 + + -16.2875 -3.11229 0 0 0 -0.509254 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -17.3228 -5.14571 0 0 0 -2.31969 + 1 1 1 + + -17.3228 -5.14571 0 0 0 -2.31969 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 0 0 -0 0 + 1 1 1 + + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0.127058 -0.012625 0.008401 -5.7e-05 0.003353 -0.002954 + 1 1 1 + + 0.127058 -0.012625 0.008401 -5.7e-05 0.003353 -0.002954 + 0.001954 -0.000866 -0.061964 -0.008321 0.009978 -0.001386 + 0.001244 0.001765 0.002452 -0.135997 -0.01007 0.000878 + 0.001244 0.001765 0.002452 0 -0 0 + + + 0.127066 -0.012614 0.008412 -2.8e-05 0.003314 -0.002936 + 0.001162 -0.002502 -0.062977 -0.012557 0.01357 -0.00402 + -0.002239 0.004917 -0.002962 -0.159433 0.012841 0.004083 + -0.002239 0.004917 -0.002962 0 -0 0 + + + 0.127057 -0.012617 0.0084 -6.9e-05 0.003501 -0.00297 + 0.002415 -0.00193 -0.061395 -0.005008 -0.011677 0.001501 + -0.003148 0.006146 -0.003368 -0.028835 -0.016126 -0.002503 + -0.000393 0.000768 -0.000421 0 -0 0 + + + 0.127059 -0.012617 0.008403 -5.8e-05 0.003458 -0.002962 + 0.002127 -0.002056 -0.061757 -0.007075 -0.00588 0.000236 + -0.001696 0.001569 -0.002879 -0.058616 -0.009069 -0.001539 + -0.001696 0.001569 -0.002879 0 -0 0 + + + -0.049765 0.051908 0.005015 -0.045565 -1.47685 -1.24358 + 0.002355 -0.002164 -0.063759 1.02255 1.31025 -0 + -0.00117 0.003147 -0.007182 0 -0 0 + -1e-06 3e-06 -7e-06 0 -0 0 + + + -0.050165 -0.076075 0.004993 1.90095 0.495873 2.6548 + 0.005449 -0.004628 -0.024591 0.926043 1.08964 0 + -0.023688 0.021829 0.247248 1.35553 -0.826371 3.14159 + -2.4e-05 2.2e-05 0.000247 0 -0 0 + + + 0.060947 -0.012566 -19432.4 0 -0 0 + 0 0 -617.145 0 -0 0 + 0 0 -9.8 0 -0 0 + 0 0 -9.8 0 -0 0 + + + 0.106509 -0.01259 0.041208 3.13937 1.12267 3.13728 + 0.002393 -0.00053 -0.06195 0.008576 0.03027 0.007711 + -0.002374 0.001791 -0.006185 -0.054285 -0.072005 -0.002259 + -0.000237 0.000179 -0.000618 0 -0 0 + + + 0.106061 -0.012586 0.040984 3.13916 1.14848 3.13713 + 0.000902 -0.000269 -0.06003 0.004974 0.030614 0.00275 + -0.001844 0.001728 0.003565 -0.052403 -0.057989 -0.04024 + -0.000184 0.000173 0.000357 0 -0 0 + + + + 0 0 10 0 -0 0 + + + + + 1.03059 2.69165 18.1021 -0 1.5578 -3.12116 + orbit + perspective + + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + 10 -9.57988 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + 10.4084 -11 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + 10.4143 -13 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -1.5566 -14 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -1 -1 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.2559 -0.59667 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.49818 -1.85004 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.74031 -3.10261 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.98256 -4.3561 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.2248 -5.60978 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -1 16 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -14.5593 -16 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.2559 -0.59667 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.49818 -1.85004 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.74031 -3.10261 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.98256 -4.3561 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.2248 -5.60978 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + 25 -15 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -25 -4 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -16.4458 -4 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -17.5466 -5 0 0 -0 0 + + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -17 -4.51853 0 0 -0 0 + + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -17.425 -4 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.2559 -0.59667 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.49818 -1.85004 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.74031 -3.10261 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.98256 -4.3561 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.2248 -5.60978 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -9 10 0 0 -0 0 + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + + -2 -0.5 0.01 0 -0 0 + + 0 + + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + 1 + + 0 + 0 + + + + -0.064 0 0.048 0 -0 0 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + 1 + + + -0.064 0 0.048 0 -0 0 + + + 0.265 0.265 0.089 + + + + + + + + + + + + + + + 10 + + + -0.064 0 0 0 -0 0 + + + model://turtlebot3_waffle/meshes/waffle_base.dae + 0.001 0.001 0.001 + + + + 0 + 0 + 0 + + + + 1 + 200 + + + + + 0 + 0.0002 + + + + + 0 + 0.0002 + + + + + 0 + 0.0002 + + + + + + + 0 + 0.017 + + + + + 0 + 0.017 + + + + + 0 + 0.017 + + + + + + + ~/out:=imu + + + + 0 + + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + 1 + + 0 + 0 + + + + -0.052 0 0.111 0 -0 0 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + 0.125 + + + -0.052 0 0.111 0 -0 0 + + + 0.0508 + 0.055 + + + + + + + + + + + + + + + 10 + + + -0.064 0 0.121 0 -0 0 + + + model://turtlebot3_waffle/meshes/lds.dae + 0.001 0.001 0.001 + + + + + 1 + 1 + -0.064 0 0.121 0 -0 0 + 5 + + + + 360 + 1 + 0 + 6.28 + + + 1 + 0 + 0 + + + + 0.32 + 20.5 + 0.015 + + + gaussian + 0 + 0.01 + + + + + ~/out:=scan + + sensor_msgs/LaserScan + base_scan + + + 0 + 0 + 0 + + + + 0 0.144 0.023 -1.57 0 0 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + 0.1 + + + 0 0.144 0.023 -1.57 0 0 + + + 0.033 + 0.018 + + + + + + 100000 + 100000 + 0 0 0 + 0 + 0 + + + + + + + + 0 + 0.2 + 100000 + 1 + 0.01 + 0.001 + + + + + 10 + + + 0 0.144 0.023 0 -0 0 + + + model://turtlebot3_waffle/meshes/left_tire.dae + 0.001 0.001 0.001 + + + + 0 + 0 + 0 + + + + 0 -0.144 0.023 -1.57 0 0 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + 0.1 + + + 0 -0.144 0.023 -1.57 0 0 + + + 0.033 + 0.018 + + + + + + 100000 + 100000 + 0 0 0 + 0 + 0 + + + + + + + + 0 + 0.2 + 100000 + 1 + 0.01 + 0.001 + + + + + 10 + + + 0 -0.144 0.023 0 -0 0 + + + model://turtlebot3_waffle/meshes/right_tire.dae + 0.001 0.001 0.001 + + + + 0 + 0 + 0 + + + -0.177 -0.064 -0.004 0 -0 0 + + 0.001 + + 1e-05 + 0 + 0 + 1e-05 + 0 + 1e-05 + + 0 0 0 0 -0 0 + + + + + 0.005 + + + + + + 0 + 0.2 + 100000 + 1 + 0.01 + 0.001 + + + + + + + + + + + 10 + + 0 + 0 + 0 + + + -0.177 0.064 -0.004 0 -0 0 + + 0.001 + + 1e-05 + 0 + 0 + 1e-05 + 0 + 1e-05 + + 0 0 0 0 -0 0 + + + + + 0.005 + + + + + + 0 + 0.2 + 100000 + 1 + 0.01 + 0.001 + + + + + + + + + + + 10 + + 0 + 0 + 0 + + + + 0.069 -0.047 0.107 0 -0 0 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + 0.035 + + + 0 0.047 -0.005 0 -0 0 + + + 0.008 0.13 0.022 + + + + + + + + + + + + + + + 10 + + 0.069 -0.047 0.107 0 -0 0 + + 1 + 5 + 0.064 -0.047 0.107 0 -0 0 + + 1.047 + + 320 + 240 + + + 0.1 + 100 + + + + + intel_realsense_r200_depth + camera_depth_frame + 0.07 + 0.001 + 5.0 + + + + 0 0.047 -0.005 0 -0 0 + + + 0.008 0.13 0.022 + + + + + + + + + + + + + + + 10 + + 0.069 -0.047 0.107 0 -0 0 + 0 + 0 + 0 + + + base_footprint + base_link + 0 0 0.01 0 -0 0 + + + base_link + wheel_left_link + 0 0.144 0.023 -1.57 0 0 + + 0 0 1 + + -1e+16 + 1e+16 + + + 0 + 0 + + + + + base_link + wheel_right_link + 0 -0.144 0.023 -1.57 0 0 + + 0 0 1 + + -1e+16 + 1e+16 + + + 0 + 0 + + + + + base_link + caster_back_right_link + + + base_link + caster_back_left_link + + + base_link + base_scan + -0.064 0 0.121 0 -0 0 + + 0 0 1 + + -1e+16 + 1e+16 + + + + + base_link + camera_link + 0.064 -0.065 0.094 0 -0 0 + + 0 0 1 + + -1e+16 + 1e+16 + + + + + + + 30 + wheel_left_joint + wheel_right_joint + 0.287 + 0.066 + 20 + 1.0 + cmd_vel + 1 + 1 + 0 + odom + odom + base_footprint + + + + ~/out:=joint_states + + 30 + wheel_left_joint + wheel_right_joint + + + + diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/worlds/ridgeback_race_empty.world b/smacc2_sm_reference_library/sm_dancebot_ue/worlds/ridgeback_race_empty.world new file mode 100644 index 000000000..3ebb28ab2 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/worlds/ridgeback_race_empty.world @@ -0,0 +1,4193 @@ + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + 0 + 0 + 0 + + + + 1 + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + + + + + + + + 10 + + + 0 + + + 0 0 1 + 100 100 + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + + + 0.001 + 1 + 1000 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + EARTH_WGS84 + 0 + 0 + 0 + 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -1 -6 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -4 1.55828 0 0 -0 0 + + + 3116 755000000 + 356 954966630 + 1605650970 323926707 + 355029 + + -2.6481 -4.01574 -0.045465 0 0 -0.74752 + 1 1 1 + + 8.54538 -6.87025 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.25439 -7.04543 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 1.49008 -7.104 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.8196 -6.78892 -0.045465 0 0 -0.74752 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.6481 -4.01574 -0.045465 0 0 -2.31822 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.78254 -6.92811 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 5.0182 -6.98712 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.8196 -6.78892 0.820535 0 0 -0.74752 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -0.070844 -6.42684 -0.045465 0 0 -2.31822 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1.3598 -5.22163 -0.045465 0 0 -2.31822 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -15.3977 -1.55916 0 0 -0 1.04571 + 1 1 1 + + -15.0829 9.98827 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.7448 4.86628 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.2984 3.15835 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -15.4433 11.2132 0 0 -0 1.04571 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -15.3977 -1.55916 0 0 0 -0.524988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.6375 8.28162 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.1908 6.57377 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -15.4433 11.2132 0.866 0 -0 1.04571 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.6146 1.4865 0 0 0 -0.524988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.5057 -0.036576 0 0 0 -0.524988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -2.60683 -10.2645 0 0 0 -0.76918 + 1 1 1 + + 8.5222 -13.3608 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.22866 -13.4213 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 1.46349 -13.4417 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.79789 -13.3071 0 0 0 -0.76918 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.60683 -10.2645 0 0 0 -2.33988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.75852 -13.3804 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 4.99332 -13.4012 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.79789 -13.3071 0.866 0 0 -0.76918 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -0.082399 -12.7309 0 0 0 -2.33988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1.34495 -11.498 0 0 0 -2.33988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3347 -5.65314 0 0 -0 1.56441 + 1 1 1 + + 4.88352 4.5315 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 8.58481 0.746571 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.81915 -0.515431 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.96327 5.4166 0 0 -0 1.56441 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.3347 -5.65314 0 0 0 -0.006293 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.11634 3.2701 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 7.35101 2.00838 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.96327 5.4166 0.866 0 -0 1.56441 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.3734 -2.1241 0 0 0 -0.006293 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.3545 -3.88863 0 0 0 -0.006293 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -16.0335 -6.41848 0 0 0 -0.7879 + 1 1 1 + + -4.96438 -9.72253 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -10.2581 -9.68398 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -12.0234 -9.67127 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -3.68791 -9.69271 0 0 0 -0.7879 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -16.0335 -6.41848 0 0 0 -2.3586 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -6.72812 -9.70918 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -8.4934 -9.69692 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -3.68791 -9.69271 0.866 0 0 -0.7879 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.5557 -8.93166 0 0 0 -2.3586 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.7949 -7.6754 0 0 0 -2.3586 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -1 16 0 0 -0 0 + 1 1 1 + + 9.2248 10.3902 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 5.49818 14.15 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 4.2559 15.4033 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.0288 9.55797 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1 16 0 0 0 -1.5707 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 7.98256 11.6439 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.74031 12.8974 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.0288 9.55797 0.866 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 2.52922 15.9838 0 0 0 -1.5707 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 0.7646 15.9914 0 0 0 -1.5707 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -14.6681 12.3809 0 0 -0 0.810038 + 1 1 1 + + -3.55516 15.9191 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -8.84779 15.8121 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -10.6121 15.7764 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.39802 15.9276 0 0 -0 0.810038 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.6681 12.3809 0 0 0 -0.760662 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -5.3197 15.8837 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -7.08412 15.8482 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.39802 15.9276 0.866 0 -0 0.810038 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -12.2231 14.926 0 0 0 -0.760662 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.4453 13.6531 0 0 0 -0.760662 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -8.62781 9.94618 0 0 -0 0.2056 + 1 1 1 + + 2.5269 6.54195 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1.88879 9.46171 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -3.36079 10.4351 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.48387 5.89136 0 0 -0 0.205596 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -8.62781 9.94618 0 0 0 -1.3651 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 1.05488 7.51562 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -0.41711 8.4891 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.48387 5.89136 0.866 0 -0 0.205596 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -5.16961 10.6508 0 0 0 -1.3651 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -6.89862 10.298 0 0 0 -1.3651 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -16.7548 -4.64388 0 0 -0 0 + 1 1 1 + + -16.7548 -4.64388 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -17.1282 -4.13814 0 0 -0 0 + 1 1 1 + + -17.1282 -4.13814 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3218 -9.21435 0 0 -0 0 + 1 1 1 + + 10.3218 -9.21435 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3067 -12.7762 0 0 -0 0 + 1 1 1 + + 10.3067 -12.7762 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3176 -7.4365 0 0 -0 0 + 1 1 1 + + 10.3176 -7.4365 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3136 -11.0025 0 0 -0 0 + 1 1 1 + + 10.3136 -11.0025 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -16.2875 -3.11229 0 0 0 -0.509254 + 1 1 1 + + -16.2875 -3.11229 0 0 0 -0.509254 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -17.3228 -5.14571 0 0 0 -2.31969 + 1 1 1 + + -17.3228 -5.14571 0 0 0 -2.31969 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 0 0 -0 0 + 1 1 1 + + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 10 0 -0 0 + + + + + 1.03059 2.69165 18.1021 -0 1.5578 -3.12116 + orbit + perspective + + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + 10 -9.57988 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + 10.4084 -11 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + 10.4143 -13 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -1.5566 -14 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -1 -1 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.2559 -0.59667 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.49818 -1.85004 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.74031 -3.10261 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.98256 -4.3561 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.2248 -5.60978 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -1 16 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -14.5593 -16 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.2559 -0.59667 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.49818 -1.85004 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.74031 -3.10261 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.98256 -4.3561 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.2248 -5.60978 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + 25 -15 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -25 -4 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -16.4458 -4 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -17.5466 -5 0 0 -0 0 + + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -17 -4.51853 0 0 -0 0 + + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -17.425 -4 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.2559 -0.59667 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.49818 -1.85004 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.74031 -3.10261 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.98256 -4.3561 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.2248 -5.60978 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -9 10 0 0 -0 0 + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + + diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/worlds/ridgeback_race_no_lidar.world b/smacc2_sm_reference_library/sm_dancebot_ue/worlds/ridgeback_race_no_lidar.world new file mode 100644 index 000000000..7a6a32d6e --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/worlds/ridgeback_race_no_lidar.world @@ -0,0 +1,4881 @@ + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + 0 + 0 + 0 + + + + 1 + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + + + + + + + + 10 + + + 0 + + + 0 0 1 + 100 100 + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + + + 0.001 + 1 + 1000 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + EARTH_WGS84 + 0 + 0 + 0 + 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -1 -6 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -4 1.55828 0 0 -0 0 + + + 3116 755000000 + 356 954966630 + 1605650970 323926707 + 355029 + + -2.6481 -4.01574 -0.045465 0 0 -0.74752 + 1 1 1 + + 8.54538 -6.87025 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.25439 -7.04543 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 1.49008 -7.104 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.8196 -6.78892 -0.045465 0 0 -0.74752 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.6481 -4.01574 -0.045465 0 0 -2.31822 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.78254 -6.92811 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 5.0182 -6.98712 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.8196 -6.78892 0.820535 0 0 -0.74752 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -0.070844 -6.42684 -0.045465 0 0 -2.31822 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1.3598 -5.22163 -0.045465 0 0 -2.31822 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -15.3977 -1.55916 0 0 -0 1.04571 + 1 1 1 + + -15.0829 9.98827 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.7448 4.86628 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.2984 3.15835 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -15.4433 11.2132 0 0 -0 1.04571 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -15.3977 -1.55916 0 0 0 -0.524988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.6375 8.28162 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.1908 6.57377 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -15.4433 11.2132 0.866 0 -0 1.04571 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.6146 1.4865 0 0 0 -0.524988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.5057 -0.036576 0 0 0 -0.524988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -2.60683 -10.2645 0 0 0 -0.76918 + 1 1 1 + + 8.5222 -13.3608 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.22866 -13.4213 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 1.46349 -13.4417 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.79789 -13.3071 0 0 0 -0.76918 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.60683 -10.2645 0 0 0 -2.33988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.75852 -13.3804 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 4.99332 -13.4012 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.79789 -13.3071 0.866 0 0 -0.76918 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -0.082399 -12.7309 0 0 0 -2.33988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1.34495 -11.498 0 0 0 -2.33988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3347 -5.65314 0 0 -0 1.56441 + 1 1 1 + + 4.88352 4.5315 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 8.58481 0.746571 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.81915 -0.515431 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.96327 5.4166 0 0 -0 1.56441 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.3347 -5.65314 0 0 0 -0.006293 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.11634 3.2701 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 7.35101 2.00838 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.96327 5.4166 0.866 0 -0 1.56441 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.3734 -2.1241 0 0 0 -0.006293 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.3545 -3.88863 0 0 0 -0.006293 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -16.0335 -6.41848 0 0 0 -0.7879 + 1 1 1 + + -4.96438 -9.72253 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -10.2581 -9.68398 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -12.0234 -9.67127 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -3.68791 -9.69271 0 0 0 -0.7879 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -16.0335 -6.41848 0 0 0 -2.3586 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -6.72812 -9.70918 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -8.4934 -9.69692 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -3.68791 -9.69271 0.866 0 0 -0.7879 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.5557 -8.93166 0 0 0 -2.3586 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.7949 -7.6754 0 0 0 -2.3586 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -1 16 0 0 -0 0 + 1 1 1 + + 9.2248 10.3902 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 5.49818 14.15 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 4.2559 15.4033 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.0288 9.55797 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1 16 0 0 0 -1.5707 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 7.98256 11.6439 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.74031 12.8974 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.0288 9.55797 0.866 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 2.52922 15.9838 0 0 0 -1.5707 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 0.7646 15.9914 0 0 0 -1.5707 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -14.6681 12.3809 0 0 -0 0.810038 + 1 1 1 + + -3.55516 15.9191 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -8.84779 15.8121 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -10.6121 15.7764 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.39802 15.9276 0 0 -0 0.810038 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.6681 12.3809 0 0 0 -0.760662 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -5.3197 15.8837 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -7.08412 15.8482 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.39802 15.9276 0.866 0 -0 0.810038 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -12.2231 14.926 0 0 0 -0.760662 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.4453 13.6531 0 0 0 -0.760662 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -8.62781 9.94618 0 0 -0 0.2056 + 1 1 1 + + 2.5269 6.54195 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1.88879 9.46171 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -3.36079 10.4351 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.48387 5.89136 0 0 -0 0.205596 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -8.62781 9.94618 0 0 0 -1.3651 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 1.05488 7.51562 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -0.41711 8.4891 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.48387 5.89136 0.866 0 -0 0.205596 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -5.16961 10.6508 0 0 0 -1.3651 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -6.89862 10.298 0 0 0 -1.3651 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -16.7548 -4.64388 0 0 -0 0 + 1 1 1 + + -16.7548 -4.64388 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -17.1282 -4.13814 0 0 -0 0 + 1 1 1 + + -17.1282 -4.13814 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3218 -9.21435 0 0 -0 0 + 1 1 1 + + 10.3218 -9.21435 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3067 -12.7762 0 0 -0 0 + 1 1 1 + + 10.3067 -12.7762 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3176 -7.4365 0 0 -0 0 + 1 1 1 + + 10.3176 -7.4365 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3136 -11.0025 0 0 -0 0 + 1 1 1 + + 10.3136 -11.0025 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -16.2875 -3.11229 0 0 0 -0.509254 + 1 1 1 + + -16.2875 -3.11229 0 0 0 -0.509254 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -17.3228 -5.14571 0 0 0 -2.31969 + 1 1 1 + + -17.3228 -5.14571 0 0 0 -2.31969 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 0 0 -0 0 + 1 1 1 + + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0.127058 -0.012625 0.008401 -5.7e-05 0.003353 -0.002954 + 1 1 1 + + 0.127058 -0.012625 0.008401 -5.7e-05 0.003353 -0.002954 + 0.001954 -0.000866 -0.061964 -0.008321 0.009978 -0.001386 + 0.001244 0.001765 0.002452 -0.135997 -0.01007 0.000878 + 0.001244 0.001765 0.002452 0 -0 0 + + + 0.127066 -0.012614 0.008412 -2.8e-05 0.003314 -0.002936 + 0.001162 -0.002502 -0.062977 -0.012557 0.01357 -0.00402 + -0.002239 0.004917 -0.002962 -0.159433 0.012841 0.004083 + -0.002239 0.004917 -0.002962 0 -0 0 + + + 0.127057 -0.012617 0.0084 -6.9e-05 0.003501 -0.00297 + 0.002415 -0.00193 -0.061395 -0.005008 -0.011677 0.001501 + -0.003148 0.006146 -0.003368 -0.028835 -0.016126 -0.002503 + -0.000393 0.000768 -0.000421 0 -0 0 + + + 0.127059 -0.012617 0.008403 -5.8e-05 0.003458 -0.002962 + 0.002127 -0.002056 -0.061757 -0.007075 -0.00588 0.000236 + -0.001696 0.001569 -0.002879 -0.058616 -0.009069 -0.001539 + -0.001696 0.001569 -0.002879 0 -0 0 + + + 0.196289 -0.059815 0.115167 -5.7e-05 0.003458 -0.002962 + 0.001535 -0.001423 -0.061086 -0.007193 -0.005937 0.000309 + -0.003549 0.013582 0.00361 -0.058136 -0.009259 -0.001821 + -0.000124 0.000475 0.000126 0 -0 0 + + + -0.049765 0.051908 0.005015 -0.045565 -1.47685 -1.24358 + 0.002355 -0.002164 -0.063759 1.02255 1.31025 -0 + -0.00117 0.003147 -0.007182 0 -0 0 + -1e-06 3e-06 -7e-06 0 -0 0 + + + -0.050165 -0.076075 0.004993 1.90095 0.495873 2.6548 + 0.005449 -0.004628 -0.024591 0.926043 1.08964 0 + -0.023688 0.021829 0.247248 1.35553 -0.826371 3.14159 + -2.4e-05 2.2e-05 0.000247 0 -0 0 + + + 0.060947 -0.012566 -19432.4 0 -0 0 + 0 0 -617.145 0 -0 0 + 0 0 -9.8 0 -0 0 + 0 0 -9.8 0 -0 0 + + + 0.106509 -0.01259 0.041208 3.13937 1.12267 3.13728 + 0.002393 -0.00053 -0.06195 0.008576 0.03027 0.007711 + -0.002374 0.001791 -0.006185 -0.054285 -0.072005 -0.002259 + -0.000237 0.000179 -0.000618 0 -0 0 + + + 0.106061 -0.012586 0.040984 3.13916 1.14848 3.13713 + 0.000902 -0.000269 -0.06003 0.004974 0.030614 0.00275 + -0.001844 0.001728 0.003565 -0.052403 -0.057989 -0.04024 + -0.000184 0.000173 0.000357 0 -0 0 + + + + 0 0 10 0 -0 0 + + + + + 1.03059 2.69165 18.1021 -0 1.5578 -3.12116 + orbit + perspective + + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + 10 -9.57988 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + 10.4084 -11 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + 10.4143 -13 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -1.5566 -14 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -1 -1 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.2559 -0.59667 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.49818 -1.85004 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.74031 -3.10261 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.98256 -4.3561 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.2248 -5.60978 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -1 16 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -14.5593 -16 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.2559 -0.59667 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.49818 -1.85004 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.74031 -3.10261 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.98256 -4.3561 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.2248 -5.60978 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + 25 -15 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -25 -4 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -16.4458 -4 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -17.5466 -5 0 0 -0 0 + + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -17 -4.51853 0 0 -0 0 + + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -17.425 -4 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.2559 -0.59667 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.49818 -1.85004 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.74031 -3.10261 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.98256 -4.3561 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.2248 -5.60978 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -9 10 0 0 -0 0 + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + + -2 -0.5 0.01 0 -0 0 + + 0 + + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + 1 + + 0 + 0 + + + + -0.064 0 0.048 0 -0 0 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + 1 + + + -0.064 0 0.048 0 -0 0 + + + 0.265 0.265 0.089 + + + + + + + + + + + + + + + 10 + + + -0.064 0 0 0 -0 0 + + + model://turtlebot3_waffle/meshes/waffle_base.dae + 0.001 0.001 0.001 + + + + 0 + 0 + 0 + + + + 1 + 200 + + + + + 0 + 0.0002 + + + + + 0 + 0.0002 + + + + + 0 + 0.0002 + + + + + + + 0 + 0.017 + + + + + 0 + 0.017 + + + + + 0 + 0.017 + + + + + + + ~/out:=imu + + + + 0 + + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + 1 + + 0 + 0 + + + + -0.052 0 0.111 0 -0 0 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + 0.125 + + + -0.052 0 0.111 0 -0 0 + + + 0.0508 + 0.055 + + + + + + + + + + + + + + + 10 + + + -0.064 0 0.121 0 -0 0 + + + model://turtlebot3_waffle/meshes/lds.dae + 0.001 0.001 0.001 + + + + + 1 + 0 + -0.064 0 0.121 0 -0 0 + 5 + + + + 360 + 1 + 0 + 6.28 + + + 1 + 0 + 0 + + + + 0.32 + 20.5 + 0.015 + + + gaussian + 0 + 0.01 + + + + + ~/out:=scan + + sensor_msgs/LaserScan + base_scan + + + 0 + 0 + 0 + + + + 0 0.144 0.023 -1.57 0 0 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + 0.1 + + + 0 0.144 0.023 -1.57 0 0 + + + 0.033 + 0.018 + + + + + + 100000 + 100000 + 0 0 0 + 0 + 0 + + + + + + + + 0 + 0.2 + 100000 + 1 + 0.01 + 0.001 + + + + + 10 + + + 0 0.144 0.023 0 -0 0 + + + model://turtlebot3_waffle/meshes/left_tire.dae + 0.001 0.001 0.001 + + + + 0 + 0 + 0 + + + + 0 -0.144 0.023 -1.57 0 0 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + 0.1 + + + 0 -0.144 0.023 -1.57 0 0 + + + 0.033 + 0.018 + + + + + + 100000 + 100000 + 0 0 0 + 0 + 0 + + + + + + + + 0 + 0.2 + 100000 + 1 + 0.01 + 0.001 + + + + + 10 + + + 0 -0.144 0.023 0 -0 0 + + + model://turtlebot3_waffle/meshes/right_tire.dae + 0.001 0.001 0.001 + + + + 0 + 0 + 0 + + + -0.177 -0.064 -0.004 0 -0 0 + + 0.001 + + 1e-05 + 0 + 0 + 1e-05 + 0 + 1e-05 + + 0 0 0 0 -0 0 + + + + + 0.005 + + + + + + 0 + 0.2 + 100000 + 1 + 0.01 + 0.001 + + + + + + + + + + + 10 + + 0 + 0 + 0 + + + -0.177 0.064 -0.004 0 -0 0 + + 0.001 + + 1e-05 + 0 + 0 + 1e-05 + 0 + 1e-05 + + 0 0 0 0 -0 0 + + + + + 0.005 + + + + + + 0 + 0.2 + 100000 + 1 + 0.01 + 0.001 + + + + + + + + + + + 10 + + 0 + 0 + 0 + + + + 0.069 -0.047 0.107 0 -0 0 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + 0.035 + + + 0 0.047 -0.005 0 -0 0 + + + 0.008 0.13 0.022 + + + + + + + + + + + + + + + 10 + + 0.069 -0.047 0.107 0 -0 0 + + 1 + 5 + 0.064 -0.047 0.107 0 -0 0 + + 1.047 + + 320 + 240 + + + 0.1 + 100 + + + + + intel_realsense_r200_depth + camera_depth_frame + 0.07 + 0.001 + 5.0 + + + + 0 0.047 -0.005 0 -0 0 + + + 0.008 0.13 0.022 + + + + + + + + + + + + + + + 10 + + 0.069 -0.047 0.107 0 -0 0 + 0 + 0 + 0 + + + base_footprint + base_link + 0 0 0.01 0 -0 0 + + + base_link + wheel_left_link + 0 0.144 0.023 -1.57 0 0 + + 0 0 1 + + -1e+16 + 1e+16 + + + 0 + 0 + + + + + base_link + wheel_right_link + 0 -0.144 0.023 -1.57 0 0 + + 0 0 1 + + -1e+16 + 1e+16 + + + 0 + 0 + + + + + base_link + caster_back_right_link + + + base_link + caster_back_left_link + + + base_link + base_scan + -0.064 0 0.121 0 -0 0 + + 0 0 1 + + -1e+16 + 1e+16 + + + + + base_link + camera_link + 0.064 -0.065 0.094 0 -0 0 + + 0 0 1 + + -1e+16 + 1e+16 + + + + + camera_link + camera_rgb_frame + 0.005 0.018 0.013 0 -0 0 + + 0 0 1 + + -1e+16 + 1e+16 + + + + + + 30 + wheel_left_joint + wheel_right_joint + 0.287 + 0.066 + 20 + 1.0 + cmd_vel + 1 + 1 + 0 + odom + odom + base_footprint + + + + ~/out:=joint_states + + 30 + wheel_left_joint + wheel_right_joint + + + + From c1198221ed2b5aad5c8e93296cb77348cdf8c761 Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Sat, 10 Jun 2023 18:20:37 +0000 Subject: [PATCH 05/70] missing --- smacc2_sm_reference_library/sm_dancebot_ue/docker/.dockerignore | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/docker/.dockerignore diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/.dockerignore b/smacc2_sm_reference_library/sm_dancebot_ue/docker/.dockerignore new file mode 100644 index 000000000..e69de29bb From 9a88bdba2ef5f740af3aaf6ba90d1c98ae355fcb Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Thu, 15 Jun 2023 17:13:06 +0000 Subject: [PATCH 06/70] progress in navigation ue --- .../launch/sm_dancebot_ue_launch.py} | 2 +- .../sm_dancebot_ue/CHANGELOG.rst | 724 +++++++++--------- .../sm_dancebot_ue/docker/Dockerfile | 2 + .../docker/update_project_files.sh | 2 + .../include/sm_dancebot_ue/sm_dancebot_ue.hpp | 3 + .../states/st_acquire_sensors.hpp | 17 +- .../states/st_navigate_to_waypoints_x.hpp | 13 +- .../superstates/ss_radial_pattern_1.hpp | 2 +- .../sm_dancebot_ue/launch/bringup_launch.py | 4 +- .../launch/navigation_launch.py | 4 +- .../sm_dancebot_ue/launch/slam_launch.py | 2 +- .../launch/sm_dancebot_ue_launch.py | 246 ++++++ .../params/nav2z_client/waypoints_plan.yaml | 7 +- .../nav2z_client_turtlebot/nav2_params.yaml | 6 +- 14 files changed, 645 insertions(+), 389 deletions(-) rename smacc2_sm_reference_library/{sm_dancebot_ue/launch/sm_dance_bot_launch.py => sm_dance_bot/launch/sm_dancebot_ue_launch.py} (99%) create mode 100755 smacc2_sm_reference_library/sm_dancebot_ue/docker/update_project_files.sh create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/launch/sm_dancebot_ue_launch.py diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/launch/sm_dance_bot_launch.py b/smacc2_sm_reference_library/sm_dance_bot/launch/sm_dancebot_ue_launch.py similarity index 99% rename from smacc2_sm_reference_library/sm_dancebot_ue/launch/sm_dance_bot_launch.py rename to smacc2_sm_reference_library/sm_dance_bot/launch/sm_dancebot_ue_launch.py index 6e30a5641..7f9a595b6 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/launch/sm_dance_bot_launch.py +++ b/smacc2_sm_reference_library/sm_dance_bot/launch/sm_dancebot_ue_launch.py @@ -28,7 +28,7 @@ def generate_launch_description(): # Get the launch directory - sm_dance_bot_dir = get_package_share_directory("sm_dance_bot") + sm_dance_bot_dir = get_package_share_directory("sm_dance_bot_ue") sm_dance_bot_launch_dir = os.path.join(sm_dance_bot_dir, "launch") # Create the launch configuration variables diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/CHANGELOG.rst b/smacc2_sm_reference_library/sm_dancebot_ue/CHANGELOG.rst index 89f28db94..d4752cf81 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/CHANGELOG.rst +++ b/smacc2_sm_reference_library/sm_dancebot_ue/CHANGELOG.rst @@ -1,5 +1,5 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package sm_dance_bot +Changelog for package sm_dancebot_ue ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2.3.6 (2023-03-12) @@ -210,7 +210,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * Feature/sm aws warehouse (#94) * base for the sm_aws_aarehouse navigation @@ -244,7 +244,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * minor * minor format @@ -264,7 +264,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * minor * formatting @@ -276,14 +276,14 @@ Changelog for package sm_dance_bot Co-authored-by: Ubuntu 20-04-02-amd64 * Rename doxygen deployment workflow (#100) * minor hotfix - * sm_dance_bot visualizing turtlebot3 (#101) + * sm_dancebot_ue visualizing turtlebot3 (#101) * Feature/dance bot launch gz lidar choice (#102) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes * Feature/sm dance bot lite gazebo fixes (#104) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes @@ -292,7 +292,7 @@ Changelog for package sm_dance_bot * sm_multi_stage_1 doubling (#103) Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/sm dance bot strikes back gazebo fixes (#105) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes @@ -353,35 +353,35 @@ Changelog for package sm_dance_bot * Remove merge markers from a python file. (#119) * Feature/slam toggle and smacc deep history (#122) * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax - * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * going forward in testing sm_dancebot_ue introducing slam pausing/resuming funcionality * feature/more_sm_dance_bot_fixes * minor format * minor (#124) Co-authored-by: Ubuntu 20-04-02-amd64 - * more changes in sm_dance_bot (#125) + * more changes in sm_dancebot_ue (#125) * Move method after the method it calls. Otherwise recursion could happen. (#126) * Feature/dance bot s pattern (#128) - * more changes in sm_dance_bot - * polishing sm_dance_bot and s-pattern + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern * noticed typo Finnaly > Finally * Feature/dance bot s pattern (#129) - * more changes in sm_dance_bot - * polishing sm_dance_bot and s-pattern - * more refinement in sm_dance_bot + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * more refinement in sm_dancebot_ue * First working version of sm template and template generator. (#127) * minor tweaks (#130) Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/sm dance bot refine (#131) - * more changes in sm_dance_bot + * more changes in sm_dancebot_ue * minor * Feature/sm dance bot refine 2 (#132) - * more changes in sm_dance_bot + * more changes in sm_dancebot_ue * minor * build fix * waypoints navigator bug (#133) * minor tuning to mitigate overshot issue cases - * progress in the sm_dance_bot tests (#135) + * progress in the sm_dancebot_ue tests (#135) * some more progress on markers cleanup * minor format issues (#134) * sm_dance_bot_lite (#136) @@ -481,7 +481,7 @@ Changelog for package sm_dance_bot Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/aws navigation sm dance bot (#174) * repo dependency - * husky launch file in sm_dance_bot + * husky launch file in sm_dancebot_ue * Add dependencies for husky simulation. * Fix formatting. * Update dependencies for husky in rolling and galactic. @@ -822,7 +822,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * Feature/sm aws warehouse (#94) * base for the sm_aws_aarehouse navigation @@ -856,7 +856,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * minor * minor format @@ -876,7 +876,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * minor * formatting @@ -888,14 +888,14 @@ Changelog for package sm_dance_bot Co-authored-by: Ubuntu 20-04-02-amd64 * Rename doxygen deployment workflow (#100) * minor hotfix - * sm_dance_bot visualizing turtlebot3 (#101) + * sm_dancebot_ue visualizing turtlebot3 (#101) * Feature/dance bot launch gz lidar choice (#102) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes * Feature/sm dance bot lite gazebo fixes (#104) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes @@ -904,7 +904,7 @@ Changelog for package sm_dance_bot * sm_multi_stage_1 doubling (#103) Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/sm dance bot strikes back gazebo fixes (#105) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes @@ -965,35 +965,35 @@ Changelog for package sm_dance_bot * Remove merge markers from a python file. (#119) * Feature/slam toggle and smacc deep history (#122) * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax - * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * going forward in testing sm_dancebot_ue introducing slam pausing/resuming funcionality * feature/more_sm_dance_bot_fixes * minor format * minor (#124) Co-authored-by: Ubuntu 20-04-02-amd64 - * more changes in sm_dance_bot (#125) + * more changes in sm_dancebot_ue (#125) * Move method after the method it calls. Otherwise recursion could happen. (#126) * Feature/dance bot s pattern (#128) - * more changes in sm_dance_bot - * polishing sm_dance_bot and s-pattern + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern * noticed typo Finnaly > Finally * Feature/dance bot s pattern (#129) - * more changes in sm_dance_bot - * polishing sm_dance_bot and s-pattern - * more refinement in sm_dance_bot + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * more refinement in sm_dancebot_ue * First working version of sm template and template generator. (#127) * minor tweaks (#130) Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/sm dance bot refine (#131) - * more changes in sm_dance_bot + * more changes in sm_dancebot_ue * minor * Feature/sm dance bot refine 2 (#132) - * more changes in sm_dance_bot + * more changes in sm_dancebot_ue * minor * build fix * waypoints navigator bug (#133) * minor tuning to mitigate overshot issue cases - * progress in the sm_dance_bot tests (#135) + * progress in the sm_dancebot_ue tests (#135) * some more progress on markers cleanup * minor format issues (#134) * sm_dance_bot_lite (#136) @@ -1093,7 +1093,7 @@ Changelog for package sm_dance_bot Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/aws navigation sm dance bot (#174) * repo dependency - * husky launch file in sm_dance_bot + * husky launch file in sm_dancebot_ue * Add dependencies for husky simulation. * Fix formatting. * Update dependencies for husky in rolling and galactic. @@ -1471,7 +1471,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * Feature/sm aws warehouse (#94) * base for the sm_aws_aarehouse navigation @@ -1505,7 +1505,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * minor * minor format @@ -1525,7 +1525,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * minor * formatting @@ -1537,14 +1537,14 @@ Changelog for package sm_dance_bot Co-authored-by: Ubuntu 20-04-02-amd64 * Rename doxygen deployment workflow (#100) * minor hotfix - * sm_dance_bot visualizing turtlebot3 (#101) + * sm_dancebot_ue visualizing turtlebot3 (#101) * Feature/dance bot launch gz lidar choice (#102) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes * Feature/sm dance bot lite gazebo fixes (#104) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes @@ -1553,7 +1553,7 @@ Changelog for package sm_dance_bot * sm_multi_stage_1 doubling (#103) Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/sm dance bot strikes back gazebo fixes (#105) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes @@ -1614,35 +1614,35 @@ Changelog for package sm_dance_bot * Remove merge markers from a python file. (#119) * Feature/slam toggle and smacc deep history (#122) * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax - * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * going forward in testing sm_dancebot_ue introducing slam pausing/resuming funcionality * feature/more_sm_dance_bot_fixes * minor format * minor (#124) Co-authored-by: Ubuntu 20-04-02-amd64 - * more changes in sm_dance_bot (#125) + * more changes in sm_dancebot_ue (#125) * Move method after the method it calls. Otherwise recursion could happen. (#126) * Feature/dance bot s pattern (#128) - * more changes in sm_dance_bot - * polishing sm_dance_bot and s-pattern + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern * noticed typo Finnaly > Finally * Feature/dance bot s pattern (#129) - * more changes in sm_dance_bot - * polishing sm_dance_bot and s-pattern - * more refinement in sm_dance_bot + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * more refinement in sm_dancebot_ue * First working version of sm template and template generator. (#127) * minor tweaks (#130) Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/sm dance bot refine (#131) - * more changes in sm_dance_bot + * more changes in sm_dancebot_ue * minor * Feature/sm dance bot refine 2 (#132) - * more changes in sm_dance_bot + * more changes in sm_dancebot_ue * minor * build fix * waypoints navigator bug (#133) * minor tuning to mitigate overshot issue cases - * progress in the sm_dance_bot tests (#135) + * progress in the sm_dancebot_ue tests (#135) * some more progress on markers cleanup * minor format issues (#134) * sm_dance_bot_lite (#136) @@ -1742,7 +1742,7 @@ Changelog for package sm_dance_bot Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/aws navigation sm dance bot (#174) * repo dependency - * husky launch file in sm_dance_bot + * husky launch file in sm_dancebot_ue * Add dependencies for husky simulation. * Fix formatting. * Update dependencies for husky in rolling and galactic. @@ -2124,7 +2124,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * Feature/sm aws warehouse (#94) * base for the sm_aws_aarehouse navigation @@ -2158,7 +2158,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * minor * minor format @@ -2178,7 +2178,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * minor * formatting @@ -2190,14 +2190,14 @@ Changelog for package sm_dance_bot Co-authored-by: Ubuntu 20-04-02-amd64 * Rename doxygen deployment workflow (#100) * minor hotfix - * sm_dance_bot visualizing turtlebot3 (#101) + * sm_dancebot_ue visualizing turtlebot3 (#101) * Feature/dance bot launch gz lidar choice (#102) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes * Feature/sm dance bot lite gazebo fixes (#104) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes @@ -2206,7 +2206,7 @@ Changelog for package sm_dance_bot * sm_multi_stage_1 doubling (#103) Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/sm dance bot strikes back gazebo fixes (#105) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes @@ -2267,35 +2267,35 @@ Changelog for package sm_dance_bot * Remove merge markers from a python file. (#119) * Feature/slam toggle and smacc deep history (#122) * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax - * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * going forward in testing sm_dancebot_ue introducing slam pausing/resuming funcionality * feature/more_sm_dance_bot_fixes * minor format * minor (#124) Co-authored-by: Ubuntu 20-04-02-amd64 - * more changes in sm_dance_bot (#125) + * more changes in sm_dancebot_ue (#125) * Move method after the method it calls. Otherwise recursion could happen. (#126) * Feature/dance bot s pattern (#128) - * more changes in sm_dance_bot - * polishing sm_dance_bot and s-pattern + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern * noticed typo Finnaly > Finally * Feature/dance bot s pattern (#129) - * more changes in sm_dance_bot - * polishing sm_dance_bot and s-pattern - * more refinement in sm_dance_bot + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * more refinement in sm_dancebot_ue * First working version of sm template and template generator. (#127) * minor tweaks (#130) Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/sm dance bot refine (#131) - * more changes in sm_dance_bot + * more changes in sm_dancebot_ue * minor * Feature/sm dance bot refine 2 (#132) - * more changes in sm_dance_bot + * more changes in sm_dancebot_ue * minor * build fix * waypoints navigator bug (#133) * minor tuning to mitigate overshot issue cases - * progress in the sm_dance_bot tests (#135) + * progress in the sm_dancebot_ue tests (#135) * some more progress on markers cleanup * minor format issues (#134) * sm_dance_bot_lite (#136) @@ -2395,7 +2395,7 @@ Changelog for package sm_dance_bot Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/aws navigation sm dance bot (#174) * repo dependency - * husky launch file in sm_dance_bot + * husky launch file in sm_dancebot_ue * Add dependencies for husky simulation. * Fix formatting. * Update dependencies for husky in rolling and galactic. @@ -2780,7 +2780,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * Feature/sm aws warehouse (#94) * base for the sm_aws_aarehouse navigation @@ -2814,7 +2814,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * minor * minor format @@ -2834,7 +2834,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * minor * formatting @@ -2846,14 +2846,14 @@ Changelog for package sm_dance_bot Co-authored-by: Ubuntu 20-04-02-amd64 * Rename doxygen deployment workflow (#100) * minor hotfix - * sm_dance_bot visualizing turtlebot3 (#101) + * sm_dancebot_ue visualizing turtlebot3 (#101) * Feature/dance bot launch gz lidar choice (#102) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes * Feature/sm dance bot lite gazebo fixes (#104) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes @@ -2862,7 +2862,7 @@ Changelog for package sm_dance_bot * sm_multi_stage_1 doubling (#103) Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/sm dance bot strikes back gazebo fixes (#105) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes @@ -2923,35 +2923,35 @@ Changelog for package sm_dance_bot * Remove merge markers from a python file. (#119) * Feature/slam toggle and smacc deep history (#122) * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax - * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * going forward in testing sm_dancebot_ue introducing slam pausing/resuming funcionality * feature/more_sm_dance_bot_fixes * minor format * minor (#124) Co-authored-by: Ubuntu 20-04-02-amd64 - * more changes in sm_dance_bot (#125) + * more changes in sm_dancebot_ue (#125) * Move method after the method it calls. Otherwise recursion could happen. (#126) * Feature/dance bot s pattern (#128) - * more changes in sm_dance_bot - * polishing sm_dance_bot and s-pattern + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern * noticed typo Finnaly > Finally * Feature/dance bot s pattern (#129) - * more changes in sm_dance_bot - * polishing sm_dance_bot and s-pattern - * more refinement in sm_dance_bot + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * more refinement in sm_dancebot_ue * First working version of sm template and template generator. (#127) * minor tweaks (#130) Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/sm dance bot refine (#131) - * more changes in sm_dance_bot + * more changes in sm_dancebot_ue * minor * Feature/sm dance bot refine 2 (#132) - * more changes in sm_dance_bot + * more changes in sm_dancebot_ue * minor * build fix * waypoints navigator bug (#133) * minor tuning to mitigate overshot issue cases - * progress in the sm_dance_bot tests (#135) + * progress in the sm_dancebot_ue tests (#135) * some more progress on markers cleanup * minor format issues (#134) * sm_dance_bot_lite (#136) @@ -3051,7 +3051,7 @@ Changelog for package sm_dance_bot Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/aws navigation sm dance bot (#174) * repo dependency - * husky launch file in sm_dance_bot + * husky launch file in sm_dancebot_ue * Add dependencies for husky simulation. * Fix formatting. * Update dependencies for husky in rolling and galactic. @@ -3435,7 +3435,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * Feature/sm aws warehouse (#94) * base for the sm_aws_aarehouse navigation @@ -3469,7 +3469,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * minor * minor format @@ -3489,7 +3489,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * minor * formatting @@ -3501,14 +3501,14 @@ Changelog for package sm_dance_bot Co-authored-by: Ubuntu 20-04-02-amd64 * Rename doxygen deployment workflow (#100) * minor hotfix - * sm_dance_bot visualizing turtlebot3 (#101) + * sm_dancebot_ue visualizing turtlebot3 (#101) * Feature/dance bot launch gz lidar choice (#102) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes * Feature/sm dance bot lite gazebo fixes (#104) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes @@ -3517,7 +3517,7 @@ Changelog for package sm_dance_bot * sm_multi_stage_1 doubling (#103) Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/sm dance bot strikes back gazebo fixes (#105) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes @@ -3578,35 +3578,35 @@ Changelog for package sm_dance_bot * Remove merge markers from a python file. (#119) * Feature/slam toggle and smacc deep history (#122) * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax - * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * going forward in testing sm_dancebot_ue introducing slam pausing/resuming funcionality * feature/more_sm_dance_bot_fixes * minor format * minor (#124) Co-authored-by: Ubuntu 20-04-02-amd64 - * more changes in sm_dance_bot (#125) + * more changes in sm_dancebot_ue (#125) * Move method after the method it calls. Otherwise recursion could happen. (#126) * Feature/dance bot s pattern (#128) - * more changes in sm_dance_bot - * polishing sm_dance_bot and s-pattern + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern * noticed typo Finnaly > Finally * Feature/dance bot s pattern (#129) - * more changes in sm_dance_bot - * polishing sm_dance_bot and s-pattern - * more refinement in sm_dance_bot + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * more refinement in sm_dancebot_ue * First working version of sm template and template generator. (#127) * minor tweaks (#130) Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/sm dance bot refine (#131) - * more changes in sm_dance_bot + * more changes in sm_dancebot_ue * minor * Feature/sm dance bot refine 2 (#132) - * more changes in sm_dance_bot + * more changes in sm_dancebot_ue * minor * build fix * waypoints navigator bug (#133) * minor tuning to mitigate overshot issue cases - * progress in the sm_dance_bot tests (#135) + * progress in the sm_dancebot_ue tests (#135) * some more progress on markers cleanup * minor format issues (#134) * sm_dance_bot_lite (#136) @@ -3706,7 +3706,7 @@ Changelog for package sm_dance_bot Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/aws navigation sm dance bot (#174) * repo dependency - * husky launch file in sm_dance_bot + * husky launch file in sm_dancebot_ue * Add dependencies for husky simulation. * Fix formatting. * Update dependencies for husky in rolling and galactic. @@ -4097,7 +4097,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * Feature/sm aws warehouse (#94) * base for the sm_aws_aarehouse navigation @@ -4131,7 +4131,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * minor * minor format @@ -4151,7 +4151,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * minor * formatting @@ -4163,14 +4163,14 @@ Changelog for package sm_dance_bot Co-authored-by: Ubuntu 20-04-02-amd64 * Rename doxygen deployment workflow (#100) * minor hotfix - * sm_dance_bot visualizing turtlebot3 (#101) + * sm_dancebot_ue visualizing turtlebot3 (#101) * Feature/dance bot launch gz lidar choice (#102) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes * Feature/sm dance bot lite gazebo fixes (#104) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes @@ -4179,7 +4179,7 @@ Changelog for package sm_dance_bot * sm_multi_stage_1 doubling (#103) Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/sm dance bot strikes back gazebo fixes (#105) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes @@ -4240,35 +4240,35 @@ Changelog for package sm_dance_bot * Remove merge markers from a python file. (#119) * Feature/slam toggle and smacc deep history (#122) * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax - * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * going forward in testing sm_dancebot_ue introducing slam pausing/resuming funcionality * feature/more_sm_dance_bot_fixes * minor format * minor (#124) Co-authored-by: Ubuntu 20-04-02-amd64 - * more changes in sm_dance_bot (#125) + * more changes in sm_dancebot_ue (#125) * Move method after the method it calls. Otherwise recursion could happen. (#126) * Feature/dance bot s pattern (#128) - * more changes in sm_dance_bot - * polishing sm_dance_bot and s-pattern + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern * noticed typo Finnaly > Finally * Feature/dance bot s pattern (#129) - * more changes in sm_dance_bot - * polishing sm_dance_bot and s-pattern - * more refinement in sm_dance_bot + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * more refinement in sm_dancebot_ue * First working version of sm template and template generator. (#127) * minor tweaks (#130) Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/sm dance bot refine (#131) - * more changes in sm_dance_bot + * more changes in sm_dancebot_ue * minor * Feature/sm dance bot refine 2 (#132) - * more changes in sm_dance_bot + * more changes in sm_dancebot_ue * minor * build fix * waypoints navigator bug (#133) * minor tuning to mitigate overshot issue cases - * progress in the sm_dance_bot tests (#135) + * progress in the sm_dancebot_ue tests (#135) * some more progress on markers cleanup * minor format issues (#134) * sm_dance_bot_lite (#136) @@ -4368,7 +4368,7 @@ Changelog for package sm_dance_bot Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/aws navigation sm dance bot (#174) * repo dependency - * husky launch file in sm_dance_bot + * husky launch file in sm_dancebot_ue * Add dependencies for husky simulation. * Fix formatting. * Update dependencies for husky in rolling and galactic. @@ -4590,7 +4590,7 @@ Changelog for package sm_dance_bot * more on backport * more on backport * disappeared ur_msgs denis repo - * fixing sm_dance_bot examples + * fixing sm_dancebot_ue examples * working on fix of image messages for husky_barrel demo Co-authored-by: Denis Štogl Co-authored-by: Denis Štogl @@ -4811,7 +4811,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * Feature/sm aws warehouse (#94) * base for the sm_aws_aarehouse navigation @@ -4845,7 +4845,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * minor * minor format @@ -4865,7 +4865,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * minor * formatting @@ -4877,14 +4877,14 @@ Changelog for package sm_dance_bot Co-authored-by: Ubuntu 20-04-02-amd64 * Rename doxygen deployment workflow (#100) * minor hotfix - * sm_dance_bot visualizing turtlebot3 (#101) + * sm_dancebot_ue visualizing turtlebot3 (#101) * Feature/dance bot launch gz lidar choice (#102) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes * Feature/sm dance bot lite gazebo fixes (#104) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes @@ -4893,7 +4893,7 @@ Changelog for package sm_dance_bot * sm_multi_stage_1 doubling (#103) Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/sm dance bot strikes back gazebo fixes (#105) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes @@ -4954,35 +4954,35 @@ Changelog for package sm_dance_bot * Remove merge markers from a python file. (#119) * Feature/slam toggle and smacc deep history (#122) * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax - * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * going forward in testing sm_dancebot_ue introducing slam pausing/resuming funcionality * feature/more_sm_dance_bot_fixes * minor format * minor (#124) Co-authored-by: Ubuntu 20-04-02-amd64 - * more changes in sm_dance_bot (#125) + * more changes in sm_dancebot_ue (#125) * Move method after the method it calls. Otherwise recursion could happen. (#126) * Feature/dance bot s pattern (#128) - * more changes in sm_dance_bot - * polishing sm_dance_bot and s-pattern + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern * noticed typo Finnaly > Finally * Feature/dance bot s pattern (#129) - * more changes in sm_dance_bot - * polishing sm_dance_bot and s-pattern - * more refinement in sm_dance_bot + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * more refinement in sm_dancebot_ue * First working version of sm template and template generator. (#127) * minor tweaks (#130) Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/sm dance bot refine (#131) - * more changes in sm_dance_bot + * more changes in sm_dancebot_ue * minor * Feature/sm dance bot refine 2 (#132) - * more changes in sm_dance_bot + * more changes in sm_dancebot_ue * minor * build fix * waypoints navigator bug (#133) * minor tuning to mitigate overshot issue cases - * progress in the sm_dance_bot tests (#135) + * progress in the sm_dancebot_ue tests (#135) * some more progress on markers cleanup * minor format issues (#134) * sm_dance_bot_lite (#136) @@ -5082,7 +5082,7 @@ Changelog for package sm_dance_bot Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/aws navigation sm dance bot (#174) * repo dependency - * husky launch file in sm_dance_bot + * husky launch file in sm_dancebot_ue * Add dependencies for husky simulation. * Fix formatting. * Update dependencies for husky in rolling and galactic. @@ -5423,7 +5423,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * Feature/sm aws warehouse (#94) * base for the sm_aws_aarehouse navigation @@ -5457,7 +5457,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * minor * minor format @@ -5477,7 +5477,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * minor * formatting @@ -5489,14 +5489,14 @@ Changelog for package sm_dance_bot Co-authored-by: Ubuntu 20-04-02-amd64 * Rename doxygen deployment workflow (#100) * minor hotfix - * sm_dance_bot visualizing turtlebot3 (#101) + * sm_dancebot_ue visualizing turtlebot3 (#101) * Feature/dance bot launch gz lidar choice (#102) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes * Feature/sm dance bot lite gazebo fixes (#104) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes @@ -5505,7 +5505,7 @@ Changelog for package sm_dance_bot * sm_multi_stage_1 doubling (#103) Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/sm dance bot strikes back gazebo fixes (#105) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes @@ -5566,35 +5566,35 @@ Changelog for package sm_dance_bot * Remove merge markers from a python file. (#119) * Feature/slam toggle and smacc deep history (#122) * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax - * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * going forward in testing sm_dancebot_ue introducing slam pausing/resuming funcionality * feature/more_sm_dance_bot_fixes * minor format * minor (#124) Co-authored-by: Ubuntu 20-04-02-amd64 - * more changes in sm_dance_bot (#125) + * more changes in sm_dancebot_ue (#125) * Move method after the method it calls. Otherwise recursion could happen. (#126) * Feature/dance bot s pattern (#128) - * more changes in sm_dance_bot - * polishing sm_dance_bot and s-pattern + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern * noticed typo Finnaly > Finally * Feature/dance bot s pattern (#129) - * more changes in sm_dance_bot - * polishing sm_dance_bot and s-pattern - * more refinement in sm_dance_bot + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * more refinement in sm_dancebot_ue * First working version of sm template and template generator. (#127) * minor tweaks (#130) Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/sm dance bot refine (#131) - * more changes in sm_dance_bot + * more changes in sm_dancebot_ue * minor * Feature/sm dance bot refine 2 (#132) - * more changes in sm_dance_bot + * more changes in sm_dancebot_ue * minor * build fix * waypoints navigator bug (#133) * minor tuning to mitigate overshot issue cases - * progress in the sm_dance_bot tests (#135) + * progress in the sm_dancebot_ue tests (#135) * some more progress on markers cleanup * minor format issues (#134) * sm_dance_bot_lite (#136) @@ -5694,7 +5694,7 @@ Changelog for package sm_dance_bot Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/aws navigation sm dance bot (#174) * repo dependency - * husky launch file in sm_dance_bot + * husky launch file in sm_dancebot_ue * Add dependencies for husky simulation. * Fix formatting. * Update dependencies for husky in rolling and galactic. @@ -6072,7 +6072,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * Feature/sm aws warehouse (#94) * base for the sm_aws_aarehouse navigation @@ -6106,7 +6106,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * minor * minor format @@ -6126,7 +6126,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * minor * formatting @@ -6138,14 +6138,14 @@ Changelog for package sm_dance_bot Co-authored-by: Ubuntu 20-04-02-amd64 * Rename doxygen deployment workflow (#100) * minor hotfix - * sm_dance_bot visualizing turtlebot3 (#101) + * sm_dancebot_ue visualizing turtlebot3 (#101) * Feature/dance bot launch gz lidar choice (#102) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes * Feature/sm dance bot lite gazebo fixes (#104) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes @@ -6154,7 +6154,7 @@ Changelog for package sm_dance_bot * sm_multi_stage_1 doubling (#103) Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/sm dance bot strikes back gazebo fixes (#105) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes @@ -6215,35 +6215,35 @@ Changelog for package sm_dance_bot * Remove merge markers from a python file. (#119) * Feature/slam toggle and smacc deep history (#122) * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax - * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * going forward in testing sm_dancebot_ue introducing slam pausing/resuming funcionality * feature/more_sm_dance_bot_fixes * minor format * minor (#124) Co-authored-by: Ubuntu 20-04-02-amd64 - * more changes in sm_dance_bot (#125) + * more changes in sm_dancebot_ue (#125) * Move method after the method it calls. Otherwise recursion could happen. (#126) * Feature/dance bot s pattern (#128) - * more changes in sm_dance_bot - * polishing sm_dance_bot and s-pattern + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern * noticed typo Finnaly > Finally * Feature/dance bot s pattern (#129) - * more changes in sm_dance_bot - * polishing sm_dance_bot and s-pattern - * more refinement in sm_dance_bot + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * more refinement in sm_dancebot_ue * First working version of sm template and template generator. (#127) * minor tweaks (#130) Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/sm dance bot refine (#131) - * more changes in sm_dance_bot + * more changes in sm_dancebot_ue * minor * Feature/sm dance bot refine 2 (#132) - * more changes in sm_dance_bot + * more changes in sm_dancebot_ue * minor * build fix * waypoints navigator bug (#133) * minor tuning to mitigate overshot issue cases - * progress in the sm_dance_bot tests (#135) + * progress in the sm_dancebot_ue tests (#135) * some more progress on markers cleanup * minor format issues (#134) * sm_dance_bot_lite (#136) @@ -6343,7 +6343,7 @@ Changelog for package sm_dance_bot Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/aws navigation sm dance bot (#174) * repo dependency - * husky launch file in sm_dance_bot + * husky launch file in sm_dancebot_ue * Add dependencies for husky simulation. * Fix formatting. * Update dependencies for husky in rolling and galactic. @@ -6725,7 +6725,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * Feature/sm aws warehouse (#94) * base for the sm_aws_aarehouse navigation @@ -6759,7 +6759,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * minor * minor format @@ -6779,7 +6779,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * minor * formatting @@ -6791,14 +6791,14 @@ Changelog for package sm_dance_bot Co-authored-by: Ubuntu 20-04-02-amd64 * Rename doxygen deployment workflow (#100) * minor hotfix - * sm_dance_bot visualizing turtlebot3 (#101) + * sm_dancebot_ue visualizing turtlebot3 (#101) * Feature/dance bot launch gz lidar choice (#102) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes * Feature/sm dance bot lite gazebo fixes (#104) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes @@ -6807,7 +6807,7 @@ Changelog for package sm_dance_bot * sm_multi_stage_1 doubling (#103) Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/sm dance bot strikes back gazebo fixes (#105) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes @@ -6868,35 +6868,35 @@ Changelog for package sm_dance_bot * Remove merge markers from a python file. (#119) * Feature/slam toggle and smacc deep history (#122) * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax - * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * going forward in testing sm_dancebot_ue introducing slam pausing/resuming funcionality * feature/more_sm_dance_bot_fixes * minor format * minor (#124) Co-authored-by: Ubuntu 20-04-02-amd64 - * more changes in sm_dance_bot (#125) + * more changes in sm_dancebot_ue (#125) * Move method after the method it calls. Otherwise recursion could happen. (#126) * Feature/dance bot s pattern (#128) - * more changes in sm_dance_bot - * polishing sm_dance_bot and s-pattern + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern * noticed typo Finnaly > Finally * Feature/dance bot s pattern (#129) - * more changes in sm_dance_bot - * polishing sm_dance_bot and s-pattern - * more refinement in sm_dance_bot + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * more refinement in sm_dancebot_ue * First working version of sm template and template generator. (#127) * minor tweaks (#130) Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/sm dance bot refine (#131) - * more changes in sm_dance_bot + * more changes in sm_dancebot_ue * minor * Feature/sm dance bot refine 2 (#132) - * more changes in sm_dance_bot + * more changes in sm_dancebot_ue * minor * build fix * waypoints navigator bug (#133) * minor tuning to mitigate overshot issue cases - * progress in the sm_dance_bot tests (#135) + * progress in the sm_dancebot_ue tests (#135) * some more progress on markers cleanup * minor format issues (#134) * sm_dance_bot_lite (#136) @@ -6996,7 +6996,7 @@ Changelog for package sm_dance_bot Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/aws navigation sm dance bot (#174) * repo dependency - * husky launch file in sm_dance_bot + * husky launch file in sm_dancebot_ue * Add dependencies for husky simulation. * Fix formatting. * Update dependencies for husky in rolling and galactic. @@ -7381,7 +7381,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * Feature/sm aws warehouse (#94) * base for the sm_aws_aarehouse navigation @@ -7415,7 +7415,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * minor * minor format @@ -7435,7 +7435,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * minor * formatting @@ -7447,14 +7447,14 @@ Changelog for package sm_dance_bot Co-authored-by: Ubuntu 20-04-02-amd64 * Rename doxygen deployment workflow (#100) * minor hotfix - * sm_dance_bot visualizing turtlebot3 (#101) + * sm_dancebot_ue visualizing turtlebot3 (#101) * Feature/dance bot launch gz lidar choice (#102) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes * Feature/sm dance bot lite gazebo fixes (#104) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes @@ -7463,7 +7463,7 @@ Changelog for package sm_dance_bot * sm_multi_stage_1 doubling (#103) Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/sm dance bot strikes back gazebo fixes (#105) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes @@ -7524,35 +7524,35 @@ Changelog for package sm_dance_bot * Remove merge markers from a python file. (#119) * Feature/slam toggle and smacc deep history (#122) * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax - * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * going forward in testing sm_dancebot_ue introducing slam pausing/resuming funcionality * feature/more_sm_dance_bot_fixes * minor format * minor (#124) Co-authored-by: Ubuntu 20-04-02-amd64 - * more changes in sm_dance_bot (#125) + * more changes in sm_dancebot_ue (#125) * Move method after the method it calls. Otherwise recursion could happen. (#126) * Feature/dance bot s pattern (#128) - * more changes in sm_dance_bot - * polishing sm_dance_bot and s-pattern + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern * noticed typo Finnaly > Finally * Feature/dance bot s pattern (#129) - * more changes in sm_dance_bot - * polishing sm_dance_bot and s-pattern - * more refinement in sm_dance_bot + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * more refinement in sm_dancebot_ue * First working version of sm template and template generator. (#127) * minor tweaks (#130) Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/sm dance bot refine (#131) - * more changes in sm_dance_bot + * more changes in sm_dancebot_ue * minor * Feature/sm dance bot refine 2 (#132) - * more changes in sm_dance_bot + * more changes in sm_dancebot_ue * minor * build fix * waypoints navigator bug (#133) * minor tuning to mitigate overshot issue cases - * progress in the sm_dance_bot tests (#135) + * progress in the sm_dancebot_ue tests (#135) * some more progress on markers cleanup * minor format issues (#134) * sm_dance_bot_lite (#136) @@ -7652,7 +7652,7 @@ Changelog for package sm_dance_bot Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/aws navigation sm dance bot (#174) * repo dependency - * husky launch file in sm_dance_bot + * husky launch file in sm_dancebot_ue * Add dependencies for husky simulation. * Fix formatting. * Update dependencies for husky in rolling and galactic. @@ -8036,7 +8036,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * Feature/sm aws warehouse (#94) * base for the sm_aws_aarehouse navigation @@ -8070,7 +8070,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * minor * minor format @@ -8090,7 +8090,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * minor * formatting @@ -8102,14 +8102,14 @@ Changelog for package sm_dance_bot Co-authored-by: Ubuntu 20-04-02-amd64 * Rename doxygen deployment workflow (#100) * minor hotfix - * sm_dance_bot visualizing turtlebot3 (#101) + * sm_dancebot_ue visualizing turtlebot3 (#101) * Feature/dance bot launch gz lidar choice (#102) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes * Feature/sm dance bot lite gazebo fixes (#104) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes @@ -8118,7 +8118,7 @@ Changelog for package sm_dance_bot * sm_multi_stage_1 doubling (#103) Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/sm dance bot strikes back gazebo fixes (#105) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes @@ -8179,35 +8179,35 @@ Changelog for package sm_dance_bot * Remove merge markers from a python file. (#119) * Feature/slam toggle and smacc deep history (#122) * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax - * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * going forward in testing sm_dancebot_ue introducing slam pausing/resuming funcionality * feature/more_sm_dance_bot_fixes * minor format * minor (#124) Co-authored-by: Ubuntu 20-04-02-amd64 - * more changes in sm_dance_bot (#125) + * more changes in sm_dancebot_ue (#125) * Move method after the method it calls. Otherwise recursion could happen. (#126) * Feature/dance bot s pattern (#128) - * more changes in sm_dance_bot - * polishing sm_dance_bot and s-pattern + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern * noticed typo Finnaly > Finally * Feature/dance bot s pattern (#129) - * more changes in sm_dance_bot - * polishing sm_dance_bot and s-pattern - * more refinement in sm_dance_bot + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * more refinement in sm_dancebot_ue * First working version of sm template and template generator. (#127) * minor tweaks (#130) Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/sm dance bot refine (#131) - * more changes in sm_dance_bot + * more changes in sm_dancebot_ue * minor * Feature/sm dance bot refine 2 (#132) - * more changes in sm_dance_bot + * more changes in sm_dancebot_ue * minor * build fix * waypoints navigator bug (#133) * minor tuning to mitigate overshot issue cases - * progress in the sm_dance_bot tests (#135) + * progress in the sm_dancebot_ue tests (#135) * some more progress on markers cleanup * minor format issues (#134) * sm_dance_bot_lite (#136) @@ -8307,7 +8307,7 @@ Changelog for package sm_dance_bot Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/aws navigation sm dance bot (#174) * repo dependency - * husky launch file in sm_dance_bot + * husky launch file in sm_dancebot_ue * Add dependencies for husky simulation. * Fix formatting. * Update dependencies for husky in rolling and galactic. @@ -8698,7 +8698,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * Feature/sm aws warehouse (#94) * base for the sm_aws_aarehouse navigation @@ -8732,7 +8732,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * minor * minor format @@ -8752,7 +8752,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * minor * formatting @@ -8764,14 +8764,14 @@ Changelog for package sm_dance_bot Co-authored-by: Ubuntu 20-04-02-amd64 * Rename doxygen deployment workflow (#100) * minor hotfix - * sm_dance_bot visualizing turtlebot3 (#101) + * sm_dancebot_ue visualizing turtlebot3 (#101) * Feature/dance bot launch gz lidar choice (#102) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes * Feature/sm dance bot lite gazebo fixes (#104) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes @@ -8780,7 +8780,7 @@ Changelog for package sm_dance_bot * sm_multi_stage_1 doubling (#103) Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/sm dance bot strikes back gazebo fixes (#105) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes @@ -8841,35 +8841,35 @@ Changelog for package sm_dance_bot * Remove merge markers from a python file. (#119) * Feature/slam toggle and smacc deep history (#122) * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax - * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * going forward in testing sm_dancebot_ue introducing slam pausing/resuming funcionality * feature/more_sm_dance_bot_fixes * minor format * minor (#124) Co-authored-by: Ubuntu 20-04-02-amd64 - * more changes in sm_dance_bot (#125) + * more changes in sm_dancebot_ue (#125) * Move method after the method it calls. Otherwise recursion could happen. (#126) * Feature/dance bot s pattern (#128) - * more changes in sm_dance_bot - * polishing sm_dance_bot and s-pattern + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern * noticed typo Finnaly > Finally * Feature/dance bot s pattern (#129) - * more changes in sm_dance_bot - * polishing sm_dance_bot and s-pattern - * more refinement in sm_dance_bot + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * more refinement in sm_dancebot_ue * First working version of sm template and template generator. (#127) * minor tweaks (#130) Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/sm dance bot refine (#131) - * more changes in sm_dance_bot + * more changes in sm_dancebot_ue * minor * Feature/sm dance bot refine 2 (#132) - * more changes in sm_dance_bot + * more changes in sm_dancebot_ue * minor * build fix * waypoints navigator bug (#133) * minor tuning to mitigate overshot issue cases - * progress in the sm_dance_bot tests (#135) + * progress in the sm_dancebot_ue tests (#135) * some more progress on markers cleanup * minor format issues (#134) * sm_dance_bot_lite (#136) @@ -8969,7 +8969,7 @@ Changelog for package sm_dance_bot Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/aws navigation sm dance bot (#174) * repo dependency - * husky launch file in sm_dance_bot + * husky launch file in sm_dancebot_ue * Add dependencies for husky simulation. * Fix formatting. * Update dependencies for husky in rolling and galactic. @@ -9191,7 +9191,7 @@ Changelog for package sm_dance_bot * more on backport * more on backport * disappeared ur_msgs denis repo - * fixing sm_dance_bot examples + * fixing sm_dancebot_ue examples * working on fix of image messages for husky_barrel demo Co-authored-by: Denis Štogl Co-authored-by: Denis Štogl @@ -9347,7 +9347,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * Feature/sm aws warehouse (#94) * base for the sm_aws_aarehouse navigation @@ -9381,7 +9381,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * minor * minor format @@ -9401,7 +9401,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * minor * formatting @@ -9413,14 +9413,14 @@ Changelog for package sm_dance_bot Co-authored-by: Ubuntu 20-04-02-amd64 * Rename doxygen deployment workflow (#100) * minor hotfix - * sm_dance_bot visualizing turtlebot3 (#101) + * sm_dancebot_ue visualizing turtlebot3 (#101) * Feature/dance bot launch gz lidar choice (#102) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes * Feature/sm dance bot lite gazebo fixes (#104) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes @@ -9429,7 +9429,7 @@ Changelog for package sm_dance_bot * sm_multi_stage_1 doubling (#103) Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/sm dance bot strikes back gazebo fixes (#105) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes @@ -9490,35 +9490,35 @@ Changelog for package sm_dance_bot * Remove merge markers from a python file. (#119) * Feature/slam toggle and smacc deep history (#122) * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax - * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * going forward in testing sm_dancebot_ue introducing slam pausing/resuming funcionality * feature/more_sm_dance_bot_fixes * minor format * minor (#124) Co-authored-by: Ubuntu 20-04-02-amd64 - * more changes in sm_dance_bot (#125) + * more changes in sm_dancebot_ue (#125) * Move method after the method it calls. Otherwise recursion could happen. (#126) * Feature/dance bot s pattern (#128) - * more changes in sm_dance_bot - * polishing sm_dance_bot and s-pattern + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern * noticed typo Finnaly > Finally * Feature/dance bot s pattern (#129) - * more changes in sm_dance_bot - * polishing sm_dance_bot and s-pattern - * more refinement in sm_dance_bot + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * more refinement in sm_dancebot_ue * First working version of sm template and template generator. (#127) * minor tweaks (#130) Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/sm dance bot refine (#131) - * more changes in sm_dance_bot + * more changes in sm_dancebot_ue * minor * Feature/sm dance bot refine 2 (#132) - * more changes in sm_dance_bot + * more changes in sm_dancebot_ue * minor * build fix * waypoints navigator bug (#133) * minor tuning to mitigate overshot issue cases - * progress in the sm_dance_bot tests (#135) + * progress in the sm_dancebot_ue tests (#135) * some more progress on markers cleanup * minor format issues (#134) * sm_dance_bot_lite (#136) @@ -9618,7 +9618,7 @@ Changelog for package sm_dance_bot Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/aws navigation sm dance bot (#174) * repo dependency - * husky launch file in sm_dance_bot + * husky launch file in sm_dancebot_ue * Add dependencies for husky simulation. * Fix formatting. * Update dependencies for husky in rolling and galactic. @@ -9957,7 +9957,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * Feature/sm aws warehouse (#94) * base for the sm_aws_aarehouse navigation @@ -9991,7 +9991,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * minor * minor format @@ -10011,7 +10011,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * minor * formatting @@ -10023,14 +10023,14 @@ Changelog for package sm_dance_bot Co-authored-by: Ubuntu 20-04-02-amd64 * Rename doxygen deployment workflow (#100) * minor hotfix - * sm_dance_bot visualizing turtlebot3 (#101) + * sm_dancebot_ue visualizing turtlebot3 (#101) * Feature/dance bot launch gz lidar choice (#102) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes * Feature/sm dance bot lite gazebo fixes (#104) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes @@ -10039,7 +10039,7 @@ Changelog for package sm_dance_bot * sm_multi_stage_1 doubling (#103) Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/sm dance bot strikes back gazebo fixes (#105) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes @@ -10100,35 +10100,35 @@ Changelog for package sm_dance_bot * Remove merge markers from a python file. (#119) * Feature/slam toggle and smacc deep history (#122) * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax - * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * going forward in testing sm_dancebot_ue introducing slam pausing/resuming funcionality * feature/more_sm_dance_bot_fixes * minor format * minor (#124) Co-authored-by: Ubuntu 20-04-02-amd64 - * more changes in sm_dance_bot (#125) + * more changes in sm_dancebot_ue (#125) * Move method after the method it calls. Otherwise recursion could happen. (#126) * Feature/dance bot s pattern (#128) - * more changes in sm_dance_bot - * polishing sm_dance_bot and s-pattern + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern * noticed typo Finnaly > Finally * Feature/dance bot s pattern (#129) - * more changes in sm_dance_bot - * polishing sm_dance_bot and s-pattern - * more refinement in sm_dance_bot + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * more refinement in sm_dancebot_ue * First working version of sm template and template generator. (#127) * minor tweaks (#130) Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/sm dance bot refine (#131) - * more changes in sm_dance_bot + * more changes in sm_dancebot_ue * minor * Feature/sm dance bot refine 2 (#132) - * more changes in sm_dance_bot + * more changes in sm_dancebot_ue * minor * build fix * waypoints navigator bug (#133) * minor tuning to mitigate overshot issue cases - * progress in the sm_dance_bot tests (#135) + * progress in the sm_dancebot_ue tests (#135) * some more progress on markers cleanup * minor format issues (#134) * sm_dance_bot_lite (#136) @@ -10228,7 +10228,7 @@ Changelog for package sm_dance_bot Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/aws navigation sm dance bot (#174) * repo dependency - * husky launch file in sm_dance_bot + * husky launch file in sm_dancebot_ue * Add dependencies for husky simulation. * Fix formatting. * Update dependencies for husky in rolling and galactic. @@ -10590,7 +10590,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * Feature/sm aws warehouse (#94) * base for the sm_aws_aarehouse navigation @@ -10624,7 +10624,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * minor * minor format @@ -10644,7 +10644,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * minor * formatting @@ -10656,14 +10656,14 @@ Changelog for package sm_dance_bot Co-authored-by: Ubuntu 20-04-02-amd64 * Rename doxygen deployment workflow (#100) * minor hotfix - * sm_dance_bot visualizing turtlebot3 (#101) + * sm_dancebot_ue visualizing turtlebot3 (#101) * Feature/dance bot launch gz lidar choice (#102) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes * Feature/sm dance bot lite gazebo fixes (#104) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes @@ -10672,7 +10672,7 @@ Changelog for package sm_dance_bot * sm_multi_stage_1 doubling (#103) Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/sm dance bot strikes back gazebo fixes (#105) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes @@ -10733,35 +10733,35 @@ Changelog for package sm_dance_bot * Remove merge markers from a python file. (#119) * Feature/slam toggle and smacc deep history (#122) * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax - * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * going forward in testing sm_dancebot_ue introducing slam pausing/resuming funcionality * feature/more_sm_dance_bot_fixes * minor format * minor (#124) Co-authored-by: Ubuntu 20-04-02-amd64 - * more changes in sm_dance_bot (#125) + * more changes in sm_dancebot_ue (#125) * Move method after the method it calls. Otherwise recursion could happen. (#126) * Feature/dance bot s pattern (#128) - * more changes in sm_dance_bot - * polishing sm_dance_bot and s-pattern + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern * noticed typo Finnaly > Finally * Feature/dance bot s pattern (#129) - * more changes in sm_dance_bot - * polishing sm_dance_bot and s-pattern - * more refinement in sm_dance_bot + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * more refinement in sm_dancebot_ue * First working version of sm template and template generator. (#127) * minor tweaks (#130) Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/sm dance bot refine (#131) - * more changes in sm_dance_bot + * more changes in sm_dancebot_ue * minor * Feature/sm dance bot refine 2 (#132) - * more changes in sm_dance_bot + * more changes in sm_dancebot_ue * minor * build fix * waypoints navigator bug (#133) * minor tuning to mitigate overshot issue cases - * progress in the sm_dance_bot tests (#135) + * progress in the sm_dancebot_ue tests (#135) * some more progress on markers cleanup * minor format issues (#134) * sm_dance_bot_lite (#136) @@ -10861,7 +10861,7 @@ Changelog for package sm_dance_bot Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/aws navigation sm dance bot (#174) * repo dependency - * husky launch file in sm_dance_bot + * husky launch file in sm_dancebot_ue * Add dependencies for husky simulation. * Fix formatting. * Update dependencies for husky in rolling and galactic. @@ -11243,7 +11243,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * Feature/sm aws warehouse (#94) * base for the sm_aws_aarehouse navigation @@ -11277,7 +11277,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * minor * minor format @@ -11297,7 +11297,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * minor * formatting @@ -11309,14 +11309,14 @@ Changelog for package sm_dance_bot Co-authored-by: Ubuntu 20-04-02-amd64 * Rename doxygen deployment workflow (#100) * minor hotfix - * sm_dance_bot visualizing turtlebot3 (#101) + * sm_dancebot_ue visualizing turtlebot3 (#101) * Feature/dance bot launch gz lidar choice (#102) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes * Feature/sm dance bot lite gazebo fixes (#104) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes @@ -11325,7 +11325,7 @@ Changelog for package sm_dance_bot * sm_multi_stage_1 doubling (#103) Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/sm dance bot strikes back gazebo fixes (#105) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes @@ -11386,35 +11386,35 @@ Changelog for package sm_dance_bot * Remove merge markers from a python file. (#119) * Feature/slam toggle and smacc deep history (#122) * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax - * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * going forward in testing sm_dancebot_ue introducing slam pausing/resuming funcionality * feature/more_sm_dance_bot_fixes * minor format * minor (#124) Co-authored-by: Ubuntu 20-04-02-amd64 - * more changes in sm_dance_bot (#125) + * more changes in sm_dancebot_ue (#125) * Move method after the method it calls. Otherwise recursion could happen. (#126) * Feature/dance bot s pattern (#128) - * more changes in sm_dance_bot - * polishing sm_dance_bot and s-pattern + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern * noticed typo Finnaly > Finally * Feature/dance bot s pattern (#129) - * more changes in sm_dance_bot - * polishing sm_dance_bot and s-pattern - * more refinement in sm_dance_bot + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * more refinement in sm_dancebot_ue * First working version of sm template and template generator. (#127) * minor tweaks (#130) Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/sm dance bot refine (#131) - * more changes in sm_dance_bot + * more changes in sm_dancebot_ue * minor * Feature/sm dance bot refine 2 (#132) - * more changes in sm_dance_bot + * more changes in sm_dancebot_ue * minor * build fix * waypoints navigator bug (#133) * minor tuning to mitigate overshot issue cases - * progress in the sm_dance_bot tests (#135) + * progress in the sm_dancebot_ue tests (#135) * some more progress on markers cleanup * minor format issues (#134) * sm_dance_bot_lite (#136) @@ -11514,7 +11514,7 @@ Changelog for package sm_dance_bot Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/aws navigation sm dance bot (#174) * repo dependency - * husky launch file in sm_dance_bot + * husky launch file in sm_dancebot_ue * Add dependencies for husky simulation. * Fix formatting. * Update dependencies for husky in rolling and galactic. @@ -11897,7 +11897,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * Feature/sm aws warehouse (#94) * base for the sm_aws_aarehouse navigation @@ -11931,7 +11931,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * minor * minor format @@ -11951,7 +11951,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * minor * formatting @@ -11963,14 +11963,14 @@ Changelog for package sm_dance_bot Co-authored-by: Ubuntu 20-04-02-amd64 * Rename doxygen deployment workflow (#100) * minor hotfix - * sm_dance_bot visualizing turtlebot3 (#101) + * sm_dancebot_ue visualizing turtlebot3 (#101) * Feature/dance bot launch gz lidar choice (#102) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes * Feature/sm dance bot lite gazebo fixes (#104) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes @@ -11979,7 +11979,7 @@ Changelog for package sm_dance_bot * sm_multi_stage_1 doubling (#103) Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/sm dance bot strikes back gazebo fixes (#105) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes @@ -12040,35 +12040,35 @@ Changelog for package sm_dance_bot * Remove merge markers from a python file. (#119) * Feature/slam toggle and smacc deep history (#122) * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax - * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * going forward in testing sm_dancebot_ue introducing slam pausing/resuming funcionality * feature/more_sm_dance_bot_fixes * minor format * minor (#124) Co-authored-by: Ubuntu 20-04-02-amd64 - * more changes in sm_dance_bot (#125) + * more changes in sm_dancebot_ue (#125) * Move method after the method it calls. Otherwise recursion could happen. (#126) * Feature/dance bot s pattern (#128) - * more changes in sm_dance_bot - * polishing sm_dance_bot and s-pattern + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern * noticed typo Finnaly > Finally * Feature/dance bot s pattern (#129) - * more changes in sm_dance_bot - * polishing sm_dance_bot and s-pattern - * more refinement in sm_dance_bot + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * more refinement in sm_dancebot_ue * First working version of sm template and template generator. (#127) * minor tweaks (#130) Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/sm dance bot refine (#131) - * more changes in sm_dance_bot + * more changes in sm_dancebot_ue * minor * Feature/sm dance bot refine 2 (#132) - * more changes in sm_dance_bot + * more changes in sm_dancebot_ue * minor * build fix * waypoints navigator bug (#133) * minor tuning to mitigate overshot issue cases - * progress in the sm_dance_bot tests (#135) + * progress in the sm_dancebot_ue tests (#135) * some more progress on markers cleanup * minor format issues (#134) * sm_dance_bot_lite (#136) @@ -12168,7 +12168,7 @@ Changelog for package sm_dance_bot Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/aws navigation sm dance bot (#174) * repo dependency - * husky launch file in sm_dance_bot + * husky launch file in sm_dancebot_ue * Add dependencies for husky simulation. * Fix formatting. * Update dependencies for husky in rolling and galactic. @@ -12569,7 +12569,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * Feature/sm aws warehouse (#94) * base for the sm_aws_aarehouse navigation @@ -12603,7 +12603,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * minor * minor format @@ -12623,7 +12623,7 @@ Changelog for package sm_dance_bot * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait * progress in aws navigation demo * minor format - * navigation parameters fixes on sm_dance_bot + * navigation parameters fixes on sm_dancebot_ue * minor format * minor * formatting @@ -12635,14 +12635,14 @@ Changelog for package sm_dance_bot Co-authored-by: Ubuntu 20-04-02-amd64 * Rename doxygen deployment workflow (#100) * minor hotfix - * sm_dance_bot visualizing turtlebot3 (#101) + * sm_dancebot_ue visualizing turtlebot3 (#101) * Feature/dance bot launch gz lidar choice (#102) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes * Feature/sm dance bot lite gazebo fixes (#104) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes @@ -12651,7 +12651,7 @@ Changelog for package sm_dance_bot * sm_multi_stage_1 doubling (#103) Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/sm dance bot strikes back gazebo fixes (#105) - * sm_dance_bot visualizing turtlebot3 + * sm_dancebot_ue visualizing turtlebot3 * cleaning and lidar show/hide option * cleaning files and making formatting work * more fixes @@ -12712,35 +12712,35 @@ Changelog for package sm_dance_bot * Remove merge markers from a python file. (#119) * Feature/slam toggle and smacc deep history (#122) * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax - * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * going forward in testing sm_dancebot_ue introducing slam pausing/resuming funcionality * feature/more_sm_dance_bot_fixes * minor format * minor (#124) Co-authored-by: Ubuntu 20-04-02-amd64 - * more changes in sm_dance_bot (#125) + * more changes in sm_dancebot_ue (#125) * Move method after the method it calls. Otherwise recursion could happen. (#126) * Feature/dance bot s pattern (#128) - * more changes in sm_dance_bot - * polishing sm_dance_bot and s-pattern + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern * noticed typo Finnaly > Finally * Feature/dance bot s pattern (#129) - * more changes in sm_dance_bot - * polishing sm_dance_bot and s-pattern - * more refinement in sm_dance_bot + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * more refinement in sm_dancebot_ue * First working version of sm template and template generator. (#127) * minor tweaks (#130) Co-authored-by: Ubuntu 20-04-02-amd64 * Feature/sm dance bot refine (#131) - * more changes in sm_dance_bot + * more changes in sm_dancebot_ue * minor * Feature/sm dance bot refine 2 (#132) - * more changes in sm_dance_bot + * more changes in sm_dancebot_ue * minor * build fix * waypoints navigator bug (#133) * minor tuning to mitigate overshot issue cases - * progress in the sm_dance_bot tests (#135) + * progress in the sm_dancebot_ue tests (#135) * some more progress on markers cleanup * minor format issues (#134) * sm_dance_bot_lite (#136) diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile b/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile index a6b387082..3604b03a8 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile @@ -65,6 +65,7 @@ RUN git clone --recurse-submodules https://github.com/rapyuta-robotics/turtlebot WORKDIR "/home/$USER/src/turtlebot3-UE" RUN git-lfs pull && git submodule foreach git-lfs pull ENV UE5_DIR=/home/$USER/src/UE5.1/UnrealEngine +RUN ls && echo ${UE5_DIR} RUN ./update_project_files.sh ENV UE5_DIR=/home/$USER/src/UE5.1 RUN sudo apt-get install nano @@ -106,6 +107,7 @@ COPY --chown=$USER:$USER smacc2_sm_reference_library/sm_dancebot_ue/docker/run_e RUN touch /home/$USER/src/turtlebot3-UE/COLCON_IGNORE RUN touch /home/$USER/src/UE5.1/COLCON_IGNORE WORKDIR "/home/$USER/" +RUN sudo apt-get install -y ros-humble-nav2-bringup ros-humble-rviz2 RUN bash -c "source /opt/ros/humble/setup.bash && colcon build --symlink-install" WORKDIR "/home/$USER/src/turtlebot3-UE" diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/update_project_files.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/update_project_files.sh new file mode 100755 index 000000000..5b178804c --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/update_project_files.sh @@ -0,0 +1,2 @@ +#!/bin/bash +/home/ros2_ws/src/UE5.1/Engine/Build/BatchFiles/Linux/GenerateProjectFiles.sh: \ No newline at end of file diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/sm_dancebot_ue.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/sm_dancebot_ue.hpp index 15024f8fb..0f655137b 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/sm_dancebot_ue.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/sm_dancebot_ue.hpp @@ -19,6 +19,7 @@ ******************************************************************************************************************/ #include +#include #include @@ -45,6 +46,8 @@ using namespace cl_nav2z; #include #include + + #include using namespace sm_dancebot_ue::cl_lidar; diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_acquire_sensors.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_acquire_sensors.hpp index b1f9713ec..98c2a6c54 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_acquire_sensors.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_acquire_sensors.hpp @@ -25,6 +25,8 @@ namespace sm_dancebot_ue { using namespace smacc2::default_events; +using smacc2::client_behaviors::CbSleepFor; +using namespace std::chrono_literals; // STATE DECLARATION struct StAcquireSensors : smacc2::SmaccState @@ -47,19 +49,22 @@ struct StAcquireSensors : smacc2::SmaccState(); - configure_orthogonal("Hello World!"); - configure_orthogonal(); - configure_orthogonal(Service3Command::SERVICE3_ON); - configure_orthogonal(); + // configure_orthogonal("Hello World!"); + // configure_orthogonal(); + // configure_orthogonal(Service3Command::SERVICE3_ON); + // configure_orthogonal(); configure_orthogonal(); + configure_orthogonal(5s); // Create State Reactor auto srAllSensorsReady = static_createStateReactor< SrAllEventsGo, smacc2::state_reactors::EvAllGo, mpl::list< EvTopicMessage, - EvTopicMessage, - EvCbSuccess>>(); + // EvTopicMessage, + EvCbSuccess, + EvCbSuccess + >>(); } }; } // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_to_waypoints_x.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_to_waypoints_x.hpp index 075f9fc15..702e9263d 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_to_waypoints_x.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_to_waypoints_x.hpp @@ -39,15 +39,10 @@ struct StNavigateToWaypointsX : smacc2::SmaccState, SS1::SsRadialPattern1, TRANSITION_1>, - Transition, SS2::SsRadialPattern2, TRANSITION_2>, - Transition, SS3::SsRadialPattern3, TRANSITION_3>, - Transition, SS4::SsFPattern1, TRANSITION_4>, - Transition, StNavigateToWaypointsX, TRANSITION_5>, - Transition, SS5::SsSPattern1, TRANSITION_6>, - Transition, StNavigateToWaypointsX, ABORT> - + Transition, StNavigateToWaypointsX, TRANSITION_1>, + Transition, SS1::SsRadialPattern1, TRANSITION_2>, + Transition, SS2::SsRadialPattern2, TRANSITION_3> + >reactions; // STATE FUNCTIONS diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_radial_pattern_1.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_radial_pattern_1.hpp index 92df50fd7..f8927c071 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_radial_pattern_1.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_radial_pattern_1.hpp @@ -55,7 +55,7 @@ struct SsRadialPattern1 static constexpr int total_iterations() { return 16; } static constexpr float ray_angle_increment_degree() { return 360.0 / total_iterations(); } - static constexpr float ray_length_meters() { return 4; } + static constexpr float ray_length_meters() { return 1.0; } int iteration_count = 0; diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/launch/bringup_launch.py b/smacc2_sm_reference_library/sm_dancebot_ue/launch/bringup_launch.py index 1714986e4..b3aa81173 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/launch/bringup_launch.py +++ b/smacc2_sm_reference_library/sm_dancebot_ue/launch/bringup_launch.py @@ -31,7 +31,7 @@ def generate_launch_description(): # Get the launch directory - sm_dance_bot_dir = get_package_share_directory("sm_dance_bot") + sm_dance_bot_dir = get_package_share_directory("sm_dancebot_ue") launch_dir = os.path.join(sm_dance_bot_dir, "launch") # Create the launch configuration variables @@ -65,7 +65,7 @@ def generate_launch_description(): ) declare_use_sim_time_cmd = DeclareLaunchArgument( - "use_sim_time", default_value="false", description="Use simulation (Gazebo) clock if true" + "use_sim_time", default_value="false", description="Use simulation clock if true" ) declare_params_file_cmd = DeclareLaunchArgument( diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/launch/navigation_launch.py b/smacc2_sm_reference_library/sm_dancebot_ue/launch/navigation_launch.py index 22e8eeeb9..4f71a43a8 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/launch/navigation_launch.py +++ b/smacc2_sm_reference_library/sm_dancebot_ue/launch/navigation_launch.py @@ -25,7 +25,7 @@ def generate_launch_description(): # Get the launch directory - sm_dance_bot_dir = get_package_share_directory("sm_dance_bot") + sm_dance_bot_dir = get_package_share_directory("sm_dancebot_ue") namespace = LaunchConfiguration("namespace") use_sim_time = LaunchConfiguration("use_sim_time") @@ -78,7 +78,7 @@ def generate_launch_description(): DeclareLaunchArgument( "use_sim_time", default_value="false", - description="Use simulation (Gazebo) clock if true", + description="Use simulation clock if true", ), DeclareLaunchArgument( "autostart", diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/launch/slam_launch.py b/smacc2_sm_reference_library/sm_dancebot_ue/launch/slam_launch.py index a3d1aa210..c3e3b851a 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/launch/slam_launch.py +++ b/smacc2_sm_reference_library/sm_dancebot_ue/launch/slam_launch.py @@ -31,7 +31,7 @@ def generate_launch_description(): # params_file = LaunchConfiguration("params_file") use_sim_time = LaunchConfiguration("use_sim_time") # autostart = LaunchConfiguration("autostart") - # sm_dance_bot_dir = get_package_share_directory("sm_dance_bot") + # sm_dance_bot_dir = get_package_share_directory("sm_dancebot_ue") # Variables # lifecycle_nodes = ["slam_toolbox"] diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/launch/sm_dancebot_ue_launch.py b/smacc2_sm_reference_library/sm_dancebot_ue/launch/sm_dancebot_ue_launch.py new file mode 100644 index 000000000..1f896fa13 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/launch/sm_dancebot_ue_launch.py @@ -0,0 +1,246 @@ +# 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. + +"""This is all-in-one launch script intended for use by nav2 developers.""" + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + # Get the launch directory + sm_dance_bot_dir = get_package_share_directory("sm_dancebot_ue") + sm_dance_bot_launch_dir = os.path.join(sm_dance_bot_dir, "launch") + + # Create the launch configuration variables + slam = LaunchConfiguration("slam") + namespace = LaunchConfiguration("namespace") + use_namespace = LaunchConfiguration("use_namespace") + map_yaml_file = LaunchConfiguration("map") + use_sim_time = LaunchConfiguration("use_sim_time") + gazebo_headless = LaunchConfiguration("headless") + + params_file = LaunchConfiguration("params_file") + default_nav_to_pose_bt_xml = LaunchConfiguration("default_nav_to_pose_bt_xml") + autostart = LaunchConfiguration("autostart") + show_gz_lidar = LaunchConfiguration("show_gz_lidar") + + # Launch configuration variables specific to simulation + rviz_config_file = LaunchConfiguration("rviz_config_file") + + use_robot_state_pub = LaunchConfiguration("use_robot_state_pub") + use_rviz = LaunchConfiguration("use_rviz") + + urdf = os.path.join(sm_dance_bot_dir, "urdf", "turtlebot3_waffle.urdf") + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + # TODO(orduno) Substitute with `PushNodeRemapping` + # https://github.com/ros2/launch_ros/issues/56 + remappings = [("/tf", "tf"), ("/tf_static", "tf_static")] + + # Declare the launch arguments + declare_namespace_cmd = DeclareLaunchArgument( + "namespace", default_value="", description="Top-level namespace" + ) + + declare_use_namespace_cmd = DeclareLaunchArgument( + "use_namespace", + default_value="false", + description="Whether to apply a namespace to the navigation stack", + ) + + declare_show_gz_lidar = DeclareLaunchArgument( + "show_gz_lidar", + default_value="true", + description="Whether to apply a namespace to the navigation stack", + ) + + declare_slam_cmd = DeclareLaunchArgument( + "slam", default_value="True", description="Whether run a SLAM" + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + "use_sim_time", default_value="true", description="Use simulation clock if true" + ) + + declare_gazebo_headless_cmd = DeclareLaunchArgument( + "headless", + default_value="false", + description="Use headless Gazebo if true", + ) + + declare_params_file_cmd = DeclareLaunchArgument( + "params_file", + default_value=os.path.join(sm_dance_bot_dir, "params", "nav2z_client", "nav2_params.yaml"), + description="Full path to the ROS2 parameters file to use for all launched nodes", + ) + + declare_bt_xml_cmd = DeclareLaunchArgument( + "default_nav_to_pose_bt_xml", + default_value=os.path.join( + sm_dance_bot_dir, "params", "nav2z_client", "navigation_tree.xml" + ), + description="Full path to the behavior tree xml file to use", + ) + + declare_autostart_cmd = DeclareLaunchArgument( + "autostart", default_value="true", description="Automatically startup the nav2 stack" + ) + + declare_rviz_config_file_cmd = DeclareLaunchArgument( + "rviz_config_file", + default_value=os.path.join(sm_dance_bot_dir, "rviz", "nav2_default_view.rviz"), + description="Full path to the RVIZ config file to use", + ) + + declare_use_robot_state_pub_cmd = DeclareLaunchArgument( + "use_robot_state_pub", + default_value="True", + description="Whether to start the robot state publisher", + ) + + declare_use_rviz_cmd = DeclareLaunchArgument( + "use_rviz", default_value="True", description="Whether to start RVIZ" + ) + + declare_map_yaml_cmd = DeclareLaunchArgument( + "map", + default_value=os.path.join(sm_dance_bot_dir, "maps", "turtlebot3_world.yaml"), + description="Full path to map file to load", + ) + + start_robot_state_publisher_cmd = Node( + condition=IfCondition(use_robot_state_pub), + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + namespace=namespace, + output="screen", + parameters=[{"use_sim_time": use_sim_time}], + remappings=remappings, + arguments=[urdf], + ) + + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(sm_dance_bot_launch_dir, "rviz_launch.py")), + condition=IfCondition(use_rviz), + launch_arguments={ + "namespace": "", + "use_namespace": "False", + "rviz_config": rviz_config_file, + }.items(), + ) + + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(sm_dance_bot_launch_dir, "bringup_launch.py")), + launch_arguments={ + "namespace": namespace, + "use_namespace": use_namespace, + "autostart": autostart, + "params_file": params_file, + "slam": slam, + "map": map_yaml_file, + "use_sim_time": use_sim_time, + "default_nav_to_pose_bt_xml": default_nav_to_pose_bt_xml, + }.items(), + ) + + xtermprefix = "xterm -xrm 'XTerm*scrollBar: true' -xrm 'xterm*rightScrollBar: true' -hold -geometry 1000x600 -sl 10000 -e" + + sm_dance_bot_node = Node( + package="sm_dancebot_ue", + executable="sm_dancebot_ue_node", + name="SmDanceBot", + output="screen", + prefix=xtermprefix, + parameters=[ + os.path.join( + get_package_share_directory("sm_dancebot_ue"), + "params/sm_dance_bot_config.yaml", + ) + ], + remappings=[ + # ("/odom", "/odometry/filtered"), + # ("/sm_dance_bot_2/odom_tracker/odom_tracker_path", "/odom_tracker_path"), + # ("/sm_dance_bot_2/odom_tracker/odom_tracker_stacked_path", "/odom_tracker_path_stacked") + ], + arguments=["--ros-args", "--log-level", "INFO"], + ) + + led_action_server_node = Node( + package="sm_dancebot_ue", + executable="led_action_server_node", + output="screen", + prefix=xtermprefix, + ) + + temperature_action_server = Node( + package="sm_dancebot_ue", + executable="temperature_sensor_node", + output="screen", + prefix=xtermprefix, + ) + + service3_node = Node( + package="sm_dancebot_ue", + executable="service_node_3.py", + output="screen", + prefix=xtermprefix, + parameters=[ + {"autostart": True, "node_names": ["ss", "dfa"]}, + ], + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_namespace_cmd) + ld.add_action(declare_slam_cmd) + ld.add_action(declare_map_yaml_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_gazebo_headless_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_bt_xml_cmd) + ld.add_action(declare_autostart_cmd) + ld.add_action(declare_map_yaml_cmd) + ld.add_action(declare_show_gz_lidar) + + ld.add_action(declare_rviz_config_file_cmd) + ld.add_action(declare_use_robot_state_pub_cmd) + ld.add_action(declare_use_rviz_cmd) + + ld.add_action(sm_dance_bot_node) + ld.add_action(service3_node) + ld.add_action(temperature_action_server) + ld.add_action(led_action_server_node) + + # # Add the actions to launch all of the navigation nodes + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + + return ld diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan.yaml b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan.yaml index 64adbe1b0..ea7c71250 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan.yaml +++ b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan.yaml @@ -1,8 +1,8 @@ waypoints: # POINT 0 - position: - x: 3.0 - y: -2.0 + x: 2.0 + y: 2.1 z: 0.0 orientation: x: 0.0 @@ -11,7 +11,7 @@ waypoints: w: 1.0 # POINT 1 - position: - x: -5.75 + x: 1.8 y: 3.5 z: 0.0 orientation: @@ -19,6 +19,7 @@ waypoints: y: 0.0 z: 0.0 w: 1.0 + # POINT 2 - position: x: -10.25 diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_turtlebot/nav2_params.yaml b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_turtlebot/nav2_params.yaml index 3eeaebed8..57d8d63ad 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_turtlebot/nav2_params.yaml +++ b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_turtlebot/nav2_params.yaml @@ -223,8 +223,8 @@ local_costmap: robot_base_frame: base_link use_sim_time: True rolling_window: true - width: 3 - height: 3 + width: 6 + height: 6 resolution: 0.05 robot_radius: 0.22 plugins: ["voxel_layer", "inflation_layer"] @@ -265,6 +265,8 @@ local_costmap: global_costmap: global_costmap: ros__parameters: + width: 8 + height: 8 update_frequency: 1.0 publish_frequency: 1.0 transform_tolerance: 5.0 From fa2229980ce1d2627b30f626dd3f9e8268fcb2f6 Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Fri, 16 Jun 2023 15:45:02 +0000 Subject: [PATCH 07/70] docker improvements for rta --- .../sm_dancebot_ue/docker/Dockerfile | 13 ++---- .../sm_dancebot_ue/docker/join_bash.sh | 2 +- .../sm_dancebot_ue/docker/remove_container.sh | 1 + .../docker/run_docker_bash_humble.sh | 4 +- .../sm_dancebot_ue/docker/run_editor_smacc.sh | 32 ++++++++++++-- .../sm_dancebot_ue/docker/stop_container.sh | 1 + .../launch/online_sync_launch.py | 44 ++++++++----------- .../sm_dancebot_ue/launch/slam_launch.py | 6 +-- .../sm_dancebot_ue/package.xml | 13 +++--- .../params/mapper_params_online_sync.yaml | 24 +++++----- .../params/nav2z_client/nav2_params.yaml | 36 +++++++-------- .../params/nav2z_client/waypoints_plan.yaml | 4 +- 12 files changed, 98 insertions(+), 82 deletions(-) create mode 100755 smacc2_sm_reference_library/sm_dancebot_ue/docker/remove_container.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_ue/docker/stop_container.sh diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile b/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile index 3604b03a8..b23753c19 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile @@ -80,7 +80,6 @@ ENV UE5_DIR=/home/$USER/src/UE5.1/UnrealEngine RUN make turtlebot3 RUN make turtlebot3Editor - # COPY smacc2 /home/$USER/src/SMACC2/smacc2 RUN mkdir -p /home/$USER/src/SMACC2/ WORKDIR "/home/$USER/src/SMACC2" @@ -93,25 +92,19 @@ COPY --chown=$USER:$USER smacc2_dev_tools /home/$USER/src/SMACC2/smacc2_dev_tool COPY --chown=$USER:$USER smacc2_msgs /home/$USER/src/SMACC2/smacc2_msgs #smacc2_sm_reference_library -# ADD smacc2_client_library /home/$USER/src/SMACC2_ws/src/SMACC2/smacc2_client_library -# ADD smacc2_state_reactor_library /home/$USER/src/SMACC2_ws/src/SMACC2/smacc2_state_reactor_library -# ADD smacc2_event_generator_library /home/$USER/src/SMACC2_ws/src/SMACC2/smacc2_event_generator_library -#RUN sudo chown -R $USER:$USER /home/$USER - #RUN git clone RUN rosdep update RUN rosdep install --ignore-src --from-paths . -y -r +RUN sudo apt-get install -y ros-humble-nav2-bringup ros-humble-rviz2 +RUN sudo apt-get install -y openvpn COPY --chown=$USER:$USER smacc2_sm_reference_library/sm_dancebot_ue/docker/run_editor_smacc.sh /home/$USER/src/turtlebot3-UE/run_editor_smacc.sh RUN touch /home/$USER/src/turtlebot3-UE/COLCON_IGNORE RUN touch /home/$USER/src/UE5.1/COLCON_IGNORE WORKDIR "/home/$USER/" -RUN sudo apt-get install -y ros-humble-nav2-bringup ros-humble-rviz2 RUN bash -c "source /opt/ros/humble/setup.bash && colcon build --symlink-install" WORKDIR "/home/$USER/src/turtlebot3-UE" -ENTRYPOINT /home/$USER/src/turtlebot3-UE/run_editor_smacc.sh - - +ENTRYPOINT /home/$USER/src/turtlebot3-UE/run_editor_smacc.sh \ No newline at end of file diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/join_bash.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/join_bash.sh index 0f043d36d..bc3ad61e3 100755 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/join_bash.sh +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/join_bash.sh @@ -1 +1 @@ -sudo docker exec -it $(sudo docker ps -aqf "ancestor=ue_editor_rclue:humble"| head -n 1) /bin/bash +sudo docker exec -it smacc_ue /bin/bash diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/remove_container.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/remove_container.sh new file mode 100755 index 000000000..1247aae92 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/remove_container.sh @@ -0,0 +1 @@ +sudo docker rm smacc_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_bash_humble.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_bash_humble.sh index 827fe76c8..0d35527a9 100755 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_bash_humble.sh +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_bash_humble.sh @@ -2,6 +2,6 @@ xhost + DIR="$(dirname "$(realpath "$0")")" -ROOT_DIR=`realpath $DIR/../../..` +ROOT_DIR=`realpath $DIR/../../../..` -sudo nvidia-docker run -it -e DISPLAY --network bridge -e QT_X11_NO_MITSHM=1 --privileged -v $ROOT_DIR:/home/ros2_ws/src/SMACC2 -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash +sudo nvidia-docker run --net host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name smacc_ue -v $ROOT_DIR/SMACC2:/home/ros2_ws/src/SMACC2 -v $ROOT_DIR/SMACC2_RTA:/home/ros2_ws/src/SMACC2_RTA -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_editor_smacc.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_editor_smacc.sh index 0e86cda47..a1f3053fb 100755 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_editor_smacc.sh +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_editor_smacc.sh @@ -1,3 +1,29 @@ -#!/bin/sh -source fastdds_setup.sh -./run_editor.sh \ No newline at end of file +#!/bin/bash +# Copyright 2020-2022 Rapyuta Robotics Co., Ltd. + +if [ -z "${UE5_DIR}" ]; then + printf "Please set UE5_DIR to path of UE5 folder\n" + exit 1 +fi + +DISCOVERY_SERVER=${1:-true} +CURRENT_DIR=$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" &> /dev/null && pwd) +TB3_UE_DIR=${2:-"${CURRENT_DIR}"} +# if $DISCOVERY_SERVER; then +# # Run discovery service for FastDDS +# (exec "${TB3_UE_DIR}/run_discovery_service.sh") + +# # Configure environment for FastDDS discovery +# source ${TB3_UE_DIR}/fastdds_setup.sh +# fi + +#change default level, generating DefautlEngine.ini +DEFAULT_LEVEL=${LEVEL_NAME:-"Turtlebot3_benchmark"} +DEFAULT_RATE=${FIXED_FRAME_RATE:-"100.0"} +DEFAULT_RTF=${TARGET_RTF:-"1.0"} +sed -e 's/${LEVEL_NAME}/'${DEFAULT_LEVEL}'/g' Config/DefaultEngineBase.ini > Config/DefaultEngine.ini +sed -i -e 's/${FIXED_FRAME_RATE}/'${DEFAULT_RATE}'/g' Config/DefaultEngine.ini +sed -i -e 's/${TARGET_RTF}/'${DEFAULT_RTF}'/g' Config/DefaultEngine.ini + +UE_EDITOR="${UE5_DIR}/Engine/Binaries/Linux/UnrealEditor" +(exec "$UE_EDITOR" "${TB3_UE_DIR}/turtlebot3.uproject" -norelativemousemode) diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/stop_container.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/stop_container.sh new file mode 100755 index 000000000..3290cdb0c --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/stop_container.sh @@ -0,0 +1 @@ +sudo docker stop smacc_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/launch/online_sync_launch.py b/smacc2_sm_reference_library/sm_dancebot_ue/launch/online_sync_launch.py index 054d891fd..9491dc329 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/launch/online_sync_launch.py +++ b/smacc2_sm_reference_library/sm_dancebot_ue/launch/online_sync_launch.py @@ -1,16 +1,4 @@ -# 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. +import os from launch import LaunchDescription from launch.actions import DeclareLaunchArgument @@ -20,29 +8,33 @@ def generate_launch_description(): - use_sim_time = LaunchConfiguration("use_sim_time") + use_sim_time = LaunchConfiguration('use_sim_time') + slam_params_file = LaunchConfiguration('slam_params_file') declare_use_sim_time_argument = DeclareLaunchArgument( - "use_sim_time", default_value="true", description="Use simulation/Gazebo clock" - ) - - xtermprefix = "xterm -xrm 'XTerm*scrollBar: true' -xrm 'xterm*rightScrollBar: true' -hold -geometry 1000x600 -sl 10000 -e" + 'use_sim_time', + default_value='true', + description='Use simulation/Gazebo clock') + declare_slam_params_file_cmd = DeclareLaunchArgument( + 'slam_params_file', + default_value=os.path.join(get_package_share_directory("slam_toolbox"), + 'config', 'mapper_params_online_sync.yaml'), + description='Full path to the ROS2 parameters file to use for the slam_toolbox node') start_sync_slam_toolbox_node = Node( parameters=[ - get_package_share_directory("slam_toolbox") + "/config/mapper_params_online_sync.yaml", - {"use_sim_time": use_sim_time}, + slam_params_file, + {'use_sim_time': use_sim_time} ], - package="slam_toolbox", - executable="sync_slam_toolbox_node", - name="slam_toolbox", - output="screen", - prefix=xtermprefix, - ) + package='slam_toolbox', + executable='sync_slam_toolbox_node', + name='slam_toolbox', + output='screen') ld = LaunchDescription() ld.add_action(declare_use_sim_time_argument) + ld.add_action(declare_slam_params_file_cmd) ld.add_action(start_sync_slam_toolbox_node) return ld diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/launch/slam_launch.py b/smacc2_sm_reference_library/sm_dancebot_ue/launch/slam_launch.py index c3e3b851a..0fcc432bc 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/launch/slam_launch.py +++ b/smacc2_sm_reference_library/sm_dancebot_ue/launch/slam_launch.py @@ -31,16 +31,16 @@ def generate_launch_description(): # params_file = LaunchConfiguration("params_file") use_sim_time = LaunchConfiguration("use_sim_time") # autostart = LaunchConfiguration("autostart") - # sm_dance_bot_dir = get_package_share_directory("sm_dancebot_ue") + sm_dance_bot_dir = get_package_share_directory("sm_dancebot_ue") # Variables # lifecycle_nodes = ["slam_toolbox"] # Getting directories and launch-files # bringup_dir = get_package_share_directory("nav2_bringup") - slam_toolbox_dir = get_package_share_directory("slam_toolbox") + #slam_toolbox_dir = get_package_share_directory("slam_toolbox") # slam_launch_file = os.path.join(sm_dance_bot_dir, 'launch', 'online_sync_launch.py') - slam_launch_file = os.path.join(slam_toolbox_dir, "launch", "online_sync_launch.py") + slam_launch_file = os.path.join(sm_dance_bot_dir, "launch", "online_sync_launch.py") # Create our own temporary YAML files that include substitutions # param_substitutions = {"use_sim_time": use_sim_time} diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/package.xml b/smacc2_sm_reference_library/sm_dancebot_ue/package.xml index 7fdf2487b..9ee8642c2 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/package.xml +++ b/smacc2_sm_reference_library/sm_dancebot_ue/package.xml @@ -26,11 +26,14 @@ sr_conditional visualization_msgs tf2 - - - gazebo_ros - gazebo - gazebo_plugins + + backward_global_planner + backward_local_planner + forward_global_planner + forward_local_planner + nav2z_planners_common + pure_spinning_local_planner + undo_path_global_planner rosidl_default_generators action_msgs diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/params/mapper_params_online_sync.yaml b/smacc2_sm_reference_library/sm_dancebot_ue/params/mapper_params_online_sync.yaml index 751672c49..01204f181 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/params/mapper_params_online_sync.yaml +++ b/smacc2_sm_reference_library/sm_dancebot_ue/params/mapper_params_online_sync.yaml @@ -27,7 +27,7 @@ slam_toolbox: throttle_scans: 1 transform_publish_period: 0.02 #if 0 never publishes odometry map_update_interval: 1.0 - resolution: 0.05 + resolution: 0.01 max_laser_range: 20.0 #for rastering images minimum_time_interval: 0.5 transform_timeout: 0.2 @@ -37,24 +37,24 @@ slam_toolbox: # General Parameters use_scan_matching: true - use_scan_barycenter: true + use_scan_barycenter: false minimum_travel_distance: 0.1 minimum_travel_heading: 0.1 scan_buffer_size: 10 scan_buffer_maximum_scan_distance: 10.0 - link_match_minimum_response_fine: 0.1 + link_match_minimum_response_fine: 0.1 link_scan_maximum_distance: 1.5 loop_search_maximum_distance: 3.0 - do_loop_closing: true - loop_match_minimum_chain_size: 10 - loop_match_maximum_variance_coarse: 3.0 - loop_match_minimum_response_coarse: 0.35 + do_loop_closing: true + loop_match_minimum_chain_size: 10 + loop_match_maximum_variance_coarse: 3.0 + loop_match_minimum_response_coarse: 0.35 loop_match_minimum_response_fine: 0.45 # Correlation Parameters - Correlation Parameters correlation_search_space_dimension: 0.5 correlation_search_space_resolution: 0.01 - correlation_search_space_smear_deviation: 0.1 + correlation_search_space_smear_deviation: 0.1 # Correlation Parameters - Loop Closure Parameters loop_search_space_dimension: 8.0 @@ -62,12 +62,12 @@ slam_toolbox: loop_search_space_smear_deviation: 0.03 # Scan Matcher Parameters - distance_variance_penalty: 0.5 + distance_variance_penalty: 0.5 angle_variance_penalty: 1.0 - fine_search_angle_offset: 0.00349 - coarse_search_angle_offset: 0.349 - coarse_angle_resolution: 0.0349 + fine_search_angle_offset: 0.00349 + coarse_search_angle_offset: 0.349 + coarse_angle_resolution: 0.0349 minimum_angle_penalty: 0.9 minimum_distance_penalty: 0.5 use_response_expansion: true diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/nav2_params.yaml b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/nav2_params.yaml index 3eeaebed8..8c5f6d85c 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/nav2_params.yaml +++ b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/nav2_params.yaml @@ -52,7 +52,7 @@ bt_navigator: use_sim_time: True global_frame: map robot_base_frame: base_link - odom_topic: /odom + odom_topic: /odom2 enable_groot_monitoring: True groot_zmq_publisher_port: 1666 groot_zmq_server_port: 1667 @@ -115,8 +115,8 @@ controller_server: # Goal checker parameters goal_checker: plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 0.257 - yaw_goal_tolerance: 0.165 # almost free orientation + xy_goal_tolerance: 0.457 + yaw_goal_tolerance: 0.265 # almost free orientation stateful: True backward_goal_checker: plugin: "nav2_controller::SimpleGoalChecker" @@ -145,20 +145,20 @@ controller_server: debug_trajectory_details: True min_vel_x: 0.0 min_vel_y: 0.0 - max_vel_x: 0.3 + max_vel_x: 0.4 max_vel_y: 0.0 - max_vel_theta: 1.0 + max_vel_theta: 0.5 min_speed_xy: 0.0 - max_speed_xy: 0.3 + max_speed_xy: 0.4 min_speed_theta: 0.0 # Add high threshold velocity for turtlebot 3 issue. # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 - acc_lim_x: 2.5 + acc_lim_x: 2.0 acc_lim_y: 0.0 - acc_lim_theta: 3.2 + acc_lim_theta: 1.2 decel_lim_x: -2.5 decel_lim_y: 0.0 - decel_lim_theta: -3.2 + decel_lim_theta: -1.2 vx_samples: 20 vy_samples: 5 vtheta_samples: 20 @@ -191,7 +191,7 @@ controller_server: carrot_distance: 0.3 #meters default 0.5 carrot_angular_distance: 0.3 # no constraint, max 3.1416 pure_spinning_straight_line_mode: true - max_linear_x_speed: 0.25 + max_linear_x_speed: 0.2 ForwardLocalPlanner: plugin: "cl_nav2z::forward_local_planner::ForwardLocalPlanner" transform_tolerance: 0.5 @@ -200,7 +200,7 @@ controller_server: k_betta: -1.0 carrot_distance: 0.2 #meters carrot_angular_distance: 0.5 # no constraint, max 3.1416 - max_linear_x_speed: 0.3 + max_linear_x_speed: 0.2 max_angular_z_speed: 0.2 PureSpinningLocalPlanner: plugin: "pure_spinning_local_planner::PureSpinningLocalPlanner" @@ -217,8 +217,8 @@ controller_server_rclcpp_node: local_costmap: local_costmap: ros__parameters: - update_frequency: 5.0 - publish_frequency: 2.0 + update_frequency: 10.0 + publish_frequency: 5.0 global_frame: odom robot_base_frame: base_link use_sim_time: True @@ -230,8 +230,8 @@ local_costmap: plugins: ["voxel_layer", "inflation_layer"] inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 3.0 - inflation_radius: 0.55 + cost_scaling_factor: 2.0 + inflation_radius: 0.25 voxel_layer: plugin: "nav2_costmap_2d::VoxelLayer" enabled: True @@ -265,7 +265,7 @@ local_costmap: global_costmap: global_costmap: ros__parameters: - update_frequency: 1.0 + update_frequency: 2.0 publish_frequency: 1.0 transform_tolerance: 5.0 global_frame: map @@ -294,8 +294,8 @@ global_costmap: map_subscribe_transient_local: True inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 3.0 - inflation_radius: 0.55 + cost_scaling_factor: 2.0 + inflation_radius: 0.25 always_send_full_costmap: True global_costmap_client: ros__parameters: diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan.yaml b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan.yaml index ea7c71250..04546657e 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan.yaml +++ b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan.yaml @@ -7,8 +7,8 @@ waypoints: orientation: x: 0.0 y: 0.0 - z: 0.0 - w: 1.0 + z: 0.7071068 + w: 0.7071068 # POINT 1 - position: x: 1.8 From e4456cdcc113dc9a2b591d54e44d58f1a511b002 Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Fri, 16 Jun 2023 16:03:40 +0000 Subject: [PATCH 08/70] ovpn config --- .../sm_dancebot_ue/docker/Dockerfile | 5 ++++- .../sm_dancebot_ue/docker/openvpn/login.txt | 0 .../sm_dancebot_ue/docker/openvpn/ovpnconfig.ovpn | 0 .../sm_dancebot_ue/docker/openvpn/password.conf | 0 .../sm_dancebot_ue/docker/openvpn_file_run.sh | 7 +++++++ 5 files changed, 11 insertions(+), 1 deletion(-) create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/docker/openvpn/login.txt create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/docker/openvpn/ovpnconfig.ovpn create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/docker/openvpn/password.conf create mode 100755 smacc2_sm_reference_library/sm_dancebot_ue/docker/openvpn_file_run.sh diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile b/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile index b23753c19..8f16763fe 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile @@ -105,6 +105,9 @@ RUN touch /home/$USER/src/UE5.1/COLCON_IGNORE WORKDIR "/home/$USER/" RUN bash -c "source /opt/ros/humble/setup.bash && colcon build --symlink-install" +RUN mkdir /home/$USER/ovpnconfig + +CMD openvpn --daemon --config /home/$USER/ovpnconfig/ovpnconfig.ovpn --auth-user-pass /home/$USER/ovpnconfig/login.txt --askpass /home/$USER/ovpnconfig/password.conf WORKDIR "/home/$USER/src/turtlebot3-UE" -ENTRYPOINT /home/$USER/src/turtlebot3-UE/run_editor_smacc.sh \ No newline at end of file +#ENTRYPOINT /home/$USER/src/turtlebot3-UE/run_editor_smacc.sh \ No newline at end of file diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/openvpn/login.txt b/smacc2_sm_reference_library/sm_dancebot_ue/docker/openvpn/login.txt new file mode 100644 index 000000000..e69de29bb diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/openvpn/ovpnconfig.ovpn b/smacc2_sm_reference_library/sm_dancebot_ue/docker/openvpn/ovpnconfig.ovpn new file mode 100644 index 000000000..e69de29bb diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/openvpn/password.conf b/smacc2_sm_reference_library/sm_dancebot_ue/docker/openvpn/password.conf new file mode 100644 index 000000000..e69de29bb diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/openvpn_file_run.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/openvpn_file_run.sh new file mode 100755 index 000000000..0d35527a9 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/openvpn_file_run.sh @@ -0,0 +1,7 @@ +#!/bin/sh +xhost + + +DIR="$(dirname "$(realpath "$0")")" +ROOT_DIR=`realpath $DIR/../../../..` + +sudo nvidia-docker run --net host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name smacc_ue -v $ROOT_DIR/SMACC2:/home/ros2_ws/src/SMACC2 -v $ROOT_DIR/SMACC2_RTA:/home/ros2_ws/src/SMACC2_RTA -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash From 701f85e47b49de98a54c2b53d6d3558afae5b7fb Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Fri, 16 Jun 2023 19:10:29 +0000 Subject: [PATCH 09/70] instructions --- .../sm_dancebot_ue/docker/README.md | 37 +++++++++++++++++++ 1 file changed, 37 insertions(+) create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md b/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md new file mode 100644 index 000000000..8a276d1e5 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md @@ -0,0 +1,37 @@ +## Unreal Engine Docker Image + +This repository provides a prebuilt Docker image containing Unreal Engine. Follow the instructions below to download, load, and run the image. + +### Downloading the Prebuilt Image + +Use the provided link to download the prebuilt Docker image for Unreal Engine: + +``` +wget https://www.dropbox.com/scl/fi/73dcb6whgy290xgpuduwj/unreal_editor_smacc-16th-Jun-2023.tar?dl=0&rlkey=60ueyxvtwtdmz2ujirkukjfy5 -O ue_editor_rclue.tar +``` + +### Loading the Docker Image + +Load the downloaded image into your Docker image database using the following command: + +``` +sudo docker load -i ue_editor_rclue.tar +``` + +### Running a New Container from the Image + +To run the Docker image, first, download the current SMACC2 repository. Then, navigate to the Docker folder and execute the following command: + +``` +./run_docker_bash_humble.sh +``` + +This will start a new container, and the Unreal Engine editor will automatically open in "edition mode." By clicking the "play" button, you can launch the simulation with Turtlebot topics accessible from both the container and the host computer. + +To run your current demo, follow these steps after executing `colcon build`: + +``` +ros2 launch sm_dancebot_ue sm_dancebot_ue_launch.py +``` + +Enjoy experimenting with Unreal Engine in your Docker environment! \ No newline at end of file From 1c12af92e21bed32ff9d9343c2779720b2bfc6ee Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Fri, 16 Jun 2023 19:13:01 +0000 Subject: [PATCH 10/70] formatting --- .../sm_dancebot_ue/docker/Dockerfile | 2 +- .../sm_dancebot_ue/docker/README.md | 2 +- .../docker/update_project_files.sh | 2 +- .../launch/online_sync_launch.py | 33 +++++++++---------- .../sm_dancebot_ue/launch/slam_launch.py | 2 +- 5 files changed, 20 insertions(+), 21 deletions(-) diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile b/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile index 8f16763fe..9ffe9e6a0 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile @@ -110,4 +110,4 @@ RUN mkdir /home/$USER/ovpnconfig CMD openvpn --daemon --config /home/$USER/ovpnconfig/ovpnconfig.ovpn --auth-user-pass /home/$USER/ovpnconfig/login.txt --askpass /home/$USER/ovpnconfig/password.conf WORKDIR "/home/$USER/src/turtlebot3-UE" -#ENTRYPOINT /home/$USER/src/turtlebot3-UE/run_editor_smacc.sh \ No newline at end of file +#ENTRYPOINT /home/$USER/src/turtlebot3-UE/run_editor_smacc.sh diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md b/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md index 8a276d1e5..42f91ec85 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md @@ -34,4 +34,4 @@ To run your current demo, follow these steps after executing `colcon build`: ros2 launch sm_dancebot_ue sm_dancebot_ue_launch.py ``` -Enjoy experimenting with Unreal Engine in your Docker environment! \ No newline at end of file +Enjoy experimenting with Unreal Engine in your Docker environment! diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/update_project_files.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/update_project_files.sh index 5b178804c..086a474a3 100755 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/update_project_files.sh +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/update_project_files.sh @@ -1,2 +1,2 @@ #!/bin/bash -/home/ros2_ws/src/UE5.1/Engine/Build/BatchFiles/Linux/GenerateProjectFiles.sh: \ No newline at end of file +/home/ros2_ws/src/UE5.1/Engine/Build/BatchFiles/Linux/GenerateProjectFiles.sh: diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/launch/online_sync_launch.py b/smacc2_sm_reference_library/sm_dancebot_ue/launch/online_sync_launch.py index 9491dc329..154a08cda 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/launch/online_sync_launch.py +++ b/smacc2_sm_reference_library/sm_dancebot_ue/launch/online_sync_launch.py @@ -8,28 +8,27 @@ def generate_launch_description(): - use_sim_time = LaunchConfiguration('use_sim_time') - slam_params_file = LaunchConfiguration('slam_params_file') + use_sim_time = LaunchConfiguration("use_sim_time") + slam_params_file = LaunchConfiguration("slam_params_file") declare_use_sim_time_argument = DeclareLaunchArgument( - 'use_sim_time', - default_value='true', - description='Use simulation/Gazebo clock') + "use_sim_time", default_value="true", description="Use simulation/Gazebo clock" + ) declare_slam_params_file_cmd = DeclareLaunchArgument( - 'slam_params_file', - default_value=os.path.join(get_package_share_directory("slam_toolbox"), - 'config', 'mapper_params_online_sync.yaml'), - description='Full path to the ROS2 parameters file to use for the slam_toolbox node') + "slam_params_file", + default_value=os.path.join( + get_package_share_directory("slam_toolbox"), "config", "mapper_params_online_sync.yaml" + ), + description="Full path to the ROS2 parameters file to use for the slam_toolbox node", + ) start_sync_slam_toolbox_node = Node( - parameters=[ - slam_params_file, - {'use_sim_time': use_sim_time} - ], - package='slam_toolbox', - executable='sync_slam_toolbox_node', - name='slam_toolbox', - output='screen') + parameters=[slam_params_file, {"use_sim_time": use_sim_time}], + package="slam_toolbox", + executable="sync_slam_toolbox_node", + name="slam_toolbox", + output="screen", + ) ld = LaunchDescription() diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/launch/slam_launch.py b/smacc2_sm_reference_library/sm_dancebot_ue/launch/slam_launch.py index 0fcc432bc..71846eec7 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/launch/slam_launch.py +++ b/smacc2_sm_reference_library/sm_dancebot_ue/launch/slam_launch.py @@ -38,7 +38,7 @@ def generate_launch_description(): # Getting directories and launch-files # bringup_dir = get_package_share_directory("nav2_bringup") - #slam_toolbox_dir = get_package_share_directory("slam_toolbox") + # slam_toolbox_dir = get_package_share_directory("slam_toolbox") # slam_launch_file = os.path.join(sm_dance_bot_dir, 'launch', 'online_sync_launch.py') slam_launch_file = os.path.join(sm_dance_bot_dir, "launch", "online_sync_launch.py") From 035779006efcd7b8bb141374466c84a9338224f3 Mon Sep 17 00:00:00 2001 From: brettpac Date: Sat, 17 Jun 2023 09:12:25 -0700 Subject: [PATCH 11/70] Got wget command to work --- smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md b/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md index 42f91ec85..8e383b22b 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md @@ -7,7 +7,7 @@ This repository provides a prebuilt Docker image containing Unreal Engine. Follo Use the provided link to download the prebuilt Docker image for Unreal Engine: ``` -wget https://www.dropbox.com/scl/fi/73dcb6whgy290xgpuduwj/unreal_editor_smacc-16th-Jun-2023.tar?dl=0&rlkey=60ueyxvtwtdmz2ujirkukjfy5 -O ue_editor_rclue.tar +wget -O ue_editor_rclue.tar 'https://www.dropbox.com/scl/fi/73dcb6whgy290xgpuduwj/unreal_editor_smacc-16th-Jun-2023.tar?dl=0&rlkey=60ueyxvtwtdmz2ujirkukjfy5' ``` ### Loading the Docker Image From ca039f29cd5354f4fc210ea480427823e655d47d Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Tue, 20 Jun 2023 19:27:31 +0000 Subject: [PATCH 12/70] minor --- .../sm_dancebot_ue/params/nav2z_client/nav2_params.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/nav2_params.yaml b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/nav2_params.yaml index 8c5f6d85c..3b3987700 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/nav2_params.yaml +++ b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/nav2_params.yaml @@ -145,11 +145,11 @@ controller_server: debug_trajectory_details: True min_vel_x: 0.0 min_vel_y: 0.0 - max_vel_x: 0.4 + max_vel_x: 0.2 max_vel_y: 0.0 - max_vel_theta: 0.5 + max_vel_theta: 0.2 min_speed_xy: 0.0 - max_speed_xy: 0.4 + max_speed_xy: 0.2 min_speed_theta: 0.0 # Add high threshold velocity for turtlebot 3 issue. # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 From 4a13bc93e8de7d9bf6cd8a7d12c21fd3e848e434 Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Fri, 23 Jun 2023 00:21:45 +0000 Subject: [PATCH 13/70] more changes on docker --- .../sm_dancebot_ue/docker/.gitignore | 1 + .../sm_dancebot_ue/docker/Dockerfile | 6 +- .../docker/attach_docker_container.sh | 1 + .../docker/run_docker_bash_humble.sh | 2 +- .../sm_dancebot_ue/docker/run_editor_smacc.sh | 2 + .../sm_dancebot_ue/docker/start_container.sh | 1 + .../scripts/test_oscillation.py | 105 ++++++++++++++++++ 7 files changed, 115 insertions(+), 3 deletions(-) create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/docker/.gitignore create mode 100755 smacc2_sm_reference_library/sm_dancebot_ue/docker/attach_docker_container.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_ue/docker/start_container.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_ue/scripts/test_oscillation.py diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/.gitignore b/smacc2_sm_reference_library/sm_dancebot_ue/docker/.gitignore new file mode 100644 index 000000000..167d48f57 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/.gitignore @@ -0,0 +1 @@ +UE5.1 diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile b/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile index 9ffe9e6a0..9db6364a6 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile @@ -92,10 +92,11 @@ COPY --chown=$USER:$USER smacc2_dev_tools /home/$USER/src/SMACC2/smacc2_dev_tool COPY --chown=$USER:$USER smacc2_msgs /home/$USER/src/SMACC2/smacc2_msgs #smacc2_sm_reference_library -#RUN git clone +#RUN git clone +RUN sudo apt-get update RUN rosdep update RUN rosdep install --ignore-src --from-paths . -y -r -RUN sudo apt-get install -y ros-humble-nav2-bringup ros-humble-rviz2 +RUN sudo apt-get update && sudo apt-get install -y ros-humble-nav2-bringup ros-humble-rviz2 RUN sudo apt-get install -y openvpn COPY --chown=$USER:$USER smacc2_sm_reference_library/sm_dancebot_ue/docker/run_editor_smacc.sh /home/$USER/src/turtlebot3-UE/run_editor_smacc.sh @@ -110,4 +111,5 @@ RUN mkdir /home/$USER/ovpnconfig CMD openvpn --daemon --config /home/$USER/ovpnconfig/ovpnconfig.ovpn --auth-user-pass /home/$USER/ovpnconfig/login.txt --askpass /home/$USER/ovpnconfig/password.conf WORKDIR "/home/$USER/src/turtlebot3-UE" +ENV LD_LIBRARY_PATH=/home/ros2_ws/src/turtlebot3-UE/Plugins/rclUE/ThirdParty/ros/lib/:/usr/local/cuda/lib64:/usr/lib/x86_64-linux-gnu/gazebo-11/plugins:/opt/ros/humble/opt/rviz_ogre_vendor/lib:/opt/ros/humble/lib/x86_64-linux-gnu: #ENTRYPOINT /home/$USER/src/turtlebot3-UE/run_editor_smacc.sh diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/attach_docker_container.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/attach_docker_container.sh new file mode 100755 index 000000000..37343f136 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/attach_docker_container.sh @@ -0,0 +1 @@ +sudo docker attach smacc_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_bash_humble.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_bash_humble.sh index 0d35527a9..f48f1b516 100755 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_bash_humble.sh +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_bash_humble.sh @@ -4,4 +4,4 @@ xhost + DIR="$(dirname "$(realpath "$0")")" ROOT_DIR=`realpath $DIR/../../../..` -sudo nvidia-docker run --net host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name smacc_ue -v $ROOT_DIR/SMACC2:/home/ros2_ws/src/SMACC2 -v $ROOT_DIR/SMACC2_RTA:/home/ros2_ws/src/SMACC2_RTA -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash +sudo nvidia-docker run --net host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name smacc_ue -v $ROOT_DIR/SMACC2:/home/ros2_ws/src/SMACC2 -v $ROOT_DIR/UE-Plugins:/home/ros2_ws/src/turtlebot3-UE/Plugins/RapyutaSimulationPlugins -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_editor_smacc.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_editor_smacc.sh index a1f3053fb..53745811c 100755 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_editor_smacc.sh +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_editor_smacc.sh @@ -1,6 +1,8 @@ #!/bin/bash # Copyright 2020-2022 Rapyuta Robotics Co., Ltd. +export LD_LIBRARY_PATH=$(echo "$LD_LIBRARY_PATH" | sed 's#/opt/ros/humble/lib:##') + if [ -z "${UE5_DIR}" ]; then printf "Please set UE5_DIR to path of UE5 folder\n" exit 1 diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/start_container.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/start_container.sh new file mode 100755 index 000000000..ec24e9d12 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/start_container.sh @@ -0,0 +1 @@ +sudo docker start smacc_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/scripts/test_oscillation.py b/smacc2_sm_reference_library/sm_dancebot_ue/scripts/test_oscillation.py new file mode 100755 index 000000000..2e9af4887 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/scripts/test_oscillation.py @@ -0,0 +1,105 @@ +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import Twist +from tf2_ros import TransformListener +from tf2_ros import Buffer +from tf2_ros import TransformStamped +import math + +class RotationOscillation(Node): + + def __init__(self): + super().__init__('rotation_oscillation') + self.cmd_vel_pub = self.create_publisher(Twist, 'cmd_vel', 10) + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + + def oscillate_rotation(self): + rate = self.create_rate(1) + angle = math.radians(25) + timeout = rclpy.duration.Duration(seconds=1.0) # Timeout for waiting on transforms + base_link_frame = 'base_footprint' + fixed_frame = 'odom' + + print('Waiting for transform from "fixed_frame" to "base_link_frame"...') + while not self.tf_buffer.can_transform(base_link_frame, fixed_frame, rclpy.time.Time(), timeout): + self.get_logger().info('Waiting for transform from "fixed_frame" to "base_link_frame"...') + rclpy.spin_once(self) + rate.sleep() + + self.get_logger().info('Transform available!') + + # Get initial position + print('Getting initial position...') + try: + base_link_transform = self.tf_buffer.lookup_transform('map', base_link_frame, rclpy.time.Time()) + initial_position = base_link_transform.transform.translation + except Exception as e: + self.get_logger().error('Error getting initial position: %s' % str(e)) + return + + # Rotate 25 degrees to the left + print('Rotating 25 degrees to the left...') + self.get_logger().info('Rotating 25 degrees to the left...') + twist_msg = Twist() + twist_msg.angular.z = angle + self.cmd_vel_pub.publish(twist_msg) + rclpy.spin_once(self) + + # Get position after left rotation + print('Getting position after left rotation...') + try: + print('Getting position after left rotation...') + base_link_transform = self.tf_buffer.lookup_transform('map', base_link_frame, rclpy.time.Time()) + left_rotation_position = base_link_transform.transform.translation + except Exception as e: + self.get_logger().error('Error getting position after left rotation: %s' % str(e)) + return + + # Rotate 25 degrees to the right + print('Rotating 25 degrees to the right...') + self.get_logger().info('Rotating 25 degrees to the right...') + twist_msg.angular.z = -angle + self.cmd_vel_pub.publish(twist_msg) + rclpy.spin_once(self) + + # Get position after right rotation + try: + base_link_transform = self.tf_buffer.lookup_transform('map', base_link_frame, rclpy.time.Time()) + right_rotation_position = base_link_transform.transform.translation + except Exception as e: + self.get_logger().error('Error getting position after right rotation: %s' % str(e)) + return + + # Calculate phase difference + print('Calculating phase difference...') + left_rotation_angle = math.atan2(left_rotation_position.y - initial_position.y, + left_rotation_position.x - initial_position.x) + right_rotation_angle = math.atan2(right_rotation_position.y - initial_position.y, + right_rotation_position.x - initial_position.x) + phase_difference = math.degrees(right_rotation_angle - left_rotation_angle) + + self.get_logger().info('Phase difference: %f degrees' % phase_difference) + + # Stop the robot + twist_msg.angular.z = 0.0 + self.cmd_vel_pub.publish(twist_msg) + + def shutdown(self): + self.get_logger().info('Shutting down...') + self.destroy_node() + +def main(args=None): + rclpy.init(args=args) + rotation_oscillation = RotationOscillation() + + try: + rotation_oscillation.oscillate_rotation() + except Exception as e: + rotation_oscillation.get_logger().error('Error during rotation oscillation: %s' % str(e)) + + rotation_oscillation.shutdown() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file From 5484a700e428ae38db413e7662215be9b62d5470 Mon Sep 17 00:00:00 2001 From: brettpac Date: Fri, 23 Jun 2023 16:10:40 -0700 Subject: [PATCH 14/70] Updated readme --- .../sm_dancebot_ue/docker/README.md | 20 +++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md b/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md index 8e383b22b..9359a2993 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md @@ -4,11 +4,8 @@ This repository provides a prebuilt Docker image containing Unreal Engine. Follo ### Downloading the Prebuilt Image -Use the provided link to download the prebuilt Docker image for Unreal Engine: +Download the prebuilt Docker image for Unreal Engine: -``` -wget -O ue_editor_rclue.tar 'https://www.dropbox.com/scl/fi/73dcb6whgy290xgpuduwj/unreal_editor_smacc-16th-Jun-2023.tar?dl=0&rlkey=60ueyxvtwtdmz2ujirkukjfy5' -``` ### Loading the Docker Image @@ -28,6 +25,21 @@ To run the Docker image, first, download the current SMACC2 repository. Then, na This will start a new container, and the Unreal Engine editor will automatically open in "edition mode." By clicking the "play" button, you can launch the simulation with Turtlebot topics accessible from both the container and the host computer. +``` +cd ~/ +``` +``` +source /opt/ros/humble/setup.bash +``` +``` +colcon build +``` +``` +source install/setup.bash +``` + + + To run your current demo, follow these steps after executing `colcon build`: ``` From c3aa77559f41caee0864ad588779901107706b6a Mon Sep 17 00:00:00 2001 From: brettpac Date: Fri, 23 Jun 2023 16:21:30 -0700 Subject: [PATCH 15/70] More readme updates --- .../sm_dancebot_ue/docker/README.md | 37 ++++++++++++++++--- 1 file changed, 32 insertions(+), 5 deletions(-) diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md b/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md index 9359a2993..b0ff80301 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md @@ -1,3 +1,33 @@ +## Pre Reqs +From https://www.ibm.com/docs/en/maximo-vi/8.2.0?topic=planning-installing-docker-nvidia-docker2 + +Make sure you have Docker installed... +``` +sudo apt-get update +sudo apt-get install apt-transport-https ca-certificates curl gnupg-agent software-properties-common +curl -fsSL https://download.docker.com/linux/ubuntu/gpg | sudo apt-key add - +sudo add-apt-repository "deb [arch=amd64] https://download.docker.com/linux/ubuntu bionic stable" +sudo apt-get update +sudo apt-get install docker-ce +``` +Make sure you have nvidia-docker2 installed + +``` +curl -s -L https://nvidia.github.io/nvidia-docker/gpgkey | sudo apt-key add - +distribution=$(. /etc/os-release;echo $ID$VERSION_ID) +curl -s -L https://nvidia.github.io/nvidia-docker/$distribution/nvidia-docker.list | sudo tee /etc/apt/sources.list.d/nvidia-docker.list +sudo apt-get update +sudo apt-get install nvidia-docker2 +sudo systemctl restart docker.service +``` +For each userid that will run Docker®, add the userid to the docker group: +``` +sudo usermod -a -G docker +``` +Then test.. +``` +sudo nvidia-docker run --rm nvidia/cuda:10.2-base-ubuntu18.04 nvidia-smi +``` ## Unreal Engine Docker Image This repository provides a prebuilt Docker image containing Unreal Engine. Follow the instructions below to download, load, and run the image. @@ -34,14 +64,11 @@ source /opt/ros/humble/setup.bash ``` colcon build ``` +To run your current demo, follow these steps after executing `colcon build`: + ``` source install/setup.bash ``` - - - -To run your current demo, follow these steps after executing `colcon build`: - ``` ros2 launch sm_dancebot_ue sm_dancebot_ue_launch.py ``` From b6b48f37024c8398ca6dba3e4b98a4e572d61067 Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Wed, 28 Jun 2023 17:20:25 +0000 Subject: [PATCH 16/70] second pass on docker ue, nvidia drivers and readme.md --- .../sm_dancebot_ue/docker/Dockerfile | 1 + .../sm_dancebot_ue/docker/README.md | 27 +++++++- .../sm_dancebot_ue/docker/join_editor.sh | 1 + .../docker/nvidia-driver-check.sh | 61 +++++++++++++++++++ ...humble.sh => run_docker_container_bash.sh} | 3 +- .../docker/run_docker_container_editor.sh | 8 +++ 6 files changed, 98 insertions(+), 3 deletions(-) create mode 100755 smacc2_sm_reference_library/sm_dancebot_ue/docker/join_editor.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_ue/docker/nvidia-driver-check.sh rename smacc2_sm_reference_library/sm_dancebot_ue/docker/{run_docker_bash_humble.sh => run_docker_container_bash.sh} (80%) create mode 100755 smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_editor.sh diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile b/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile index 9db6364a6..5df310cd1 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile @@ -100,6 +100,7 @@ RUN sudo apt-get update && sudo apt-get install -y ros-humble-nav2-bringup ros-h RUN sudo apt-get install -y openvpn COPY --chown=$USER:$USER smacc2_sm_reference_library/sm_dancebot_ue/docker/run_editor_smacc.sh /home/$USER/src/turtlebot3-UE/run_editor_smacc.sh +COPY --chown=$USER:$USER smacc2_sm_reference_library/sm_dancebot_ue/docker/nvidia-driver-check.sh /home/$USER/src/turtlebot3-UE/nvidia-driver-check.sh RUN touch /home/$USER/src/turtlebot3-UE/COLCON_IGNORE RUN touch /home/$USER/src/UE5.1/COLCON_IGNORE diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md b/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md index b0ff80301..f5fe4ba8f 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md @@ -50,10 +50,33 @@ sudo docker load -i ue_editor_rclue.tar To run the Docker image, first, download the current SMACC2 repository. Then, navigate to the Docker folder and execute the following command: ``` -./run_docker_bash_humble.sh +run_docker_container_bash.sh ``` -This will start a new container, and the Unreal Engine editor will automatically open in "edition mode." By clicking the "play" button, you can launch the simulation with Turtlebot topics accessible from both the container and the host computer. +This will start a new container, but the editor will not be open yet + +### Open the unreal engine editor + +Assuming the container is already running execute the following: + +``` +join_editor.sh +``` +The Unreal Engine editor will automatically open in "edition mode." By clicking the "play" button, you can launch the simulation with Turtlebot topics accessible from both the container and the host computer. + +### Join to the container via bash + +``` +join_bash.sh +``` +### Optionally Stop running container and remove + +``` +./stop_container.sh +./remove_container.sh +``` + +### Optionally Run a smacc state machine inside the container ``` cd ~/ diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/join_editor.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/join_editor.sh new file mode 100755 index 000000000..d215b6054 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/join_editor.sh @@ -0,0 +1 @@ +sudo docker exec -it smacc_ue ./run_editor_smacc.sh diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/nvidia-driver-check.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/nvidia-driver-check.sh new file mode 100755 index 000000000..b40312af6 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/nvidia-driver-check.sh @@ -0,0 +1,61 @@ +#!/bin/bash + +# String with the package description obtained from the original machine +#package_description="nvidia-driver-525/unknown,now 525.85.12-0ubuntu1 amd64 [installed]" +#DRIVER=$(apt list --installed | grep nvidia-driver) + +echo "Package description: $1" +package_description=$1 +#package_description=$DRIVER + +regex="([^/]+)/[^[:space:]]+ (.+)" +#regex="([^/]+)/([^[:space:]]+).+" +if [[ $package_description =~ $regex ]]; then + package_name="${BASH_REMATCH[1]}" + package_version="${BASH_REMATCH[2]}" +else + echo "Failed to extract package name and version from the package description." + exit 1 +fi + +echo "Package name: $package_name" +echo "Package version: $package_version" + +# Check if the package is installed +if dpkg -s "$package_name" >/dev/null 2>&1; then + echo "Package $package_name is installed." + echo "Checking if it has version $package_version." + + installed_package_description=$(apt list --installed | grep -w "$package_name") + echo "Installed package: $installed_package_description" + + if [[ $installed_package_description =~ $regex ]]; then + installed_version="${BASH_REMATCH[2]}" + fi + echo "Installed version: $installed_version" + + # Verify if the installed version matches the desired version + if [[ "$installed_version" == "$package_version" ]]; then + echo "Package $package_name-$package_version is already installed." + else + echo "WARNING: Package $package_name is installed, but it does not have the same debian package version $package_version." + fi +else + echo "Package $package_name is not installed." + DO_INSTALL=1 +fi + +if [ "$DO_INSTALL" == "1" ]; then + echo "Attempting to install package $package_name-$package_version." + # Attempt to install the package + sudo apt-get install -y "$package_name=$package_version" >/dev/null 2>&1 + + # Check if the installation was successful + if dpkg -s "$package_name" >/dev/null 2>&1; then + echo "Package $package_name-$package_version has been successfully installed." + else + echo "Unable to install package $package_name-$package_version." + fi +else + echo "Checking nvidia-driver. Nothing to do. Package $package_name-$package_version is already installed." +fi \ No newline at end of file diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_bash_humble.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_bash.sh similarity index 80% rename from smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_bash_humble.sh rename to smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_bash.sh index f48f1b516..8effa45bb 100755 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_bash_humble.sh +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_bash.sh @@ -3,5 +3,6 @@ xhost + DIR="$(dirname "$(realpath "$0")")" ROOT_DIR=`realpath $DIR/../../../..` +DRIVER=$(apt list --installed | grep nvidia-driver) -sudo nvidia-docker run --net host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name smacc_ue -v $ROOT_DIR/SMACC2:/home/ros2_ws/src/SMACC2 -v $ROOT_DIR/UE-Plugins:/home/ros2_ws/src/turtlebot3-UE/Plugins/RapyutaSimulationPlugins -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash +sudo nvidia-docker run --net host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name smacc_ue -v $ROOT_DIR/SMACC2:/home/ros2_ws/src/SMACC2 -v $ROOT_DIR/UE-Plugins:/home/ros2_ws/src/turtlebot3-UE/Plugins/RapyutaSimulationPlugins -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash -c "./nvidia-driver-check.sh '$DRIVER' && bash" diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_editor.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_editor.sh new file mode 100755 index 000000000..f8f33da20 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_editor.sh @@ -0,0 +1,8 @@ +#!/bin/sh +xhost + + +DIR="$(dirname "$(realpath "$0")")" +ROOT_DIR=`realpath $DIR/../../../..` + +DRIVER=$(apt list --installed | grep nvidia-driver) +sudo nvidia-docker run --net host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name smacc_ue -v $ROOT_DIR/SMACC2:/home/ros2_ws/src/SMACC2 -v $ROOT_DIR/UE-Plugins:/home/ros2_ws/src/turtlebot3-UE/Plugins/RapyutaSimulationPlugins -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash -c "./nvidia-driver-check.sh '$DRIVER' && ./run_editor_smacc.sh" \ No newline at end of file From d76fe8b11720cbb03cf68e2e57fb383434cdfd1c Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Wed, 28 Jun 2023 18:22:10 +0000 Subject: [PATCH 17/70] minor changes --- smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md | 3 ++- .../sm_dancebot_ue/docker/nvidia-driver-check.sh | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md b/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md index f5fe4ba8f..29f4199c3 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md @@ -45,7 +45,7 @@ Load the downloaded image into your Docker image database using the following co sudo docker load -i ue_editor_rclue.tar ``` -### Running a New Container from the Image +### Running/Creating a new container from the smacc2_editor_ue docker image To run the Docker image, first, download the current SMACC2 repository. Then, navigate to the Docker folder and execute the following command: @@ -62,6 +62,7 @@ Assuming the container is already running execute the following: ``` join_editor.sh ``` + The Unreal Engine editor will automatically open in "edition mode." By clicking the "play" button, you can launch the simulation with Turtlebot topics accessible from both the container and the host computer. ### Join to the container via bash diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/nvidia-driver-check.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/nvidia-driver-check.sh index b40312af6..3555a0fcf 100755 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/nvidia-driver-check.sh +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/nvidia-driver-check.sh @@ -39,6 +39,7 @@ if dpkg -s "$package_name" >/dev/null 2>&1; then echo "Package $package_name-$package_version is already installed." else echo "WARNING: Package $package_name is installed, but it does not have the same debian package version $package_version." + DO_INSTALL=1 fi else echo "Package $package_name is not installed." @@ -48,7 +49,7 @@ fi if [ "$DO_INSTALL" == "1" ]; then echo "Attempting to install package $package_name-$package_version." # Attempt to install the package - sudo apt-get install -y "$package_name=$package_version" >/dev/null 2>&1 + sudo apt-get install -y "$package_name" # Check if the installation was successful if dpkg -s "$package_name" >/dev/null 2>&1; then From bbb07994a7a3cc07362965b57a2ccb19bf37f372 Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Wed, 28 Jun 2023 18:23:01 +0000 Subject: [PATCH 18/70] minor changes --- .../sm_dancebot_ue/docker/nvidia-driver-check.sh | 2 +- .../sm_dancebot_ue/docker/run_docker_container_editor.sh | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/nvidia-driver-check.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/nvidia-driver-check.sh index 3555a0fcf..294fd294f 100755 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/nvidia-driver-check.sh +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/nvidia-driver-check.sh @@ -59,4 +59,4 @@ if [ "$DO_INSTALL" == "1" ]; then fi else echo "Checking nvidia-driver. Nothing to do. Package $package_name-$package_version is already installed." -fi \ No newline at end of file +fi diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_editor.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_editor.sh index f8f33da20..bacdfc1d7 100755 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_editor.sh +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_editor.sh @@ -5,4 +5,4 @@ DIR="$(dirname "$(realpath "$0")")" ROOT_DIR=`realpath $DIR/../../../..` DRIVER=$(apt list --installed | grep nvidia-driver) -sudo nvidia-docker run --net host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name smacc_ue -v $ROOT_DIR/SMACC2:/home/ros2_ws/src/SMACC2 -v $ROOT_DIR/UE-Plugins:/home/ros2_ws/src/turtlebot3-UE/Plugins/RapyutaSimulationPlugins -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash -c "./nvidia-driver-check.sh '$DRIVER' && ./run_editor_smacc.sh" \ No newline at end of file +sudo nvidia-docker run --net host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name smacc_ue -v $ROOT_DIR/SMACC2:/home/ros2_ws/src/SMACC2 -v $ROOT_DIR/UE-Plugins:/home/ros2_ws/src/turtlebot3-UE/Plugins/RapyutaSimulationPlugins -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash -c "./nvidia-driver-check.sh '$DRIVER' && ./run_editor_smacc.sh" From bc54e64a0e444a334989c8fbd86e8a11c296fee7 Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Wed, 28 Jun 2023 18:27:10 +0000 Subject: [PATCH 19/70] minor --- .../sm_dancebot_ue/docker/README.md | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md b/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md index 29f4199c3..0e7e63e8c 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md @@ -50,7 +50,7 @@ sudo docker load -i ue_editor_rclue.tar To run the Docker image, first, download the current SMACC2 repository. Then, navigate to the Docker folder and execute the following command: ``` -run_docker_container_bash.sh +./run_docker_container_bash.sh ``` This will start a new container, but the editor will not be open yet @@ -60,7 +60,7 @@ This will start a new container, but the editor will not be open yet Assuming the container is already running execute the following: ``` -join_editor.sh +./join_editor.sh ``` The Unreal Engine editor will automatically open in "edition mode." By clicking the "play" button, you can launch the simulation with Turtlebot topics accessible from both the container and the host computer. @@ -68,7 +68,7 @@ The Unreal Engine editor will automatically open in "edition mode." By clicking ### Join to the container via bash ``` -join_bash.sh +./join_bash.sh ``` ### Optionally Stop running container and remove @@ -77,7 +77,14 @@ join_bash.sh ./remove_container.sh ``` -### Optionally Run a smacc state machine inside the container +### Optionally Rebuild and run a smacc state machine inside the container + +Notice that ue_editor_rcl containers are run mapping the current smacc2 source folder so it is possible to do a mixed development between the host (to develop from vscode) and the container (to compile and run the state machines). + +Smacc2 source code is already prebuilt inside the image consecuently it is available to any container. +You could need to modify smacc2 code or examples and rebuilt it. + +If that is the case you can do the following: ``` cd ~/ From f101b4eb128cc60108d50d46f15b1f0f1500c996 Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Wed, 28 Jun 2023 18:35:04 +0000 Subject: [PATCH 20/70] minor --- .../sm_dancebot_ue/docker/README.md | 38 +++++++++++++++---- 1 file changed, 31 insertions(+), 7 deletions(-) diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md b/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md index 0e7e63e8c..20b58b016 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md @@ -45,7 +45,7 @@ Load the downloaded image into your Docker image database using the following co sudo docker load -i ue_editor_rclue.tar ``` -### Running/Creating a new container from the smacc2_editor_ue docker image +### Running/Creating a new container from the ue_editor_rcl docker image To run the Docker image, first, download the current SMACC2 repository. Then, navigate to the Docker folder and execute the following command: @@ -53,11 +53,7 @@ To run the Docker image, first, download the current SMACC2 repository. Then, na ./run_docker_container_bash.sh ``` -This will start a new container, but the editor will not be open yet - -### Open the unreal engine editor - -Assuming the container is already running execute the following: +This will create and start a new container as a daemon (should be available on restarting). This container is able to open de unreal engine editor with ROS2, but the editor will not open until you run the following command: ``` ./join_editor.sh @@ -65,18 +61,43 @@ Assuming the container is already running execute the following: The Unreal Engine editor will automatically open in "edition mode." By clicking the "play" button, you can launch the simulation with Turtlebot topics accessible from both the container and the host computer. + + +### Start/stop an already existing container from the ue_editor_rcl docker image + +``` +./start_container.sh +``` + +``` +./stop_container.sh +``` ### Join to the container via bash +You may need to enter into the docker container to debug, or test stuff from commandline. +Then you can use the following command: ``` ./join_bash.sh ``` -### Optionally Stop running container and remove +### Optionally Stopping and removing running container + +You may want to reset everyting if you messed the container. Before you create a new container you need +to stop and remove the existing one. ``` ./stop_container.sh ./remove_container.sh ``` +### Optionally Stopping and removing running container + +In the case you want to change the docker image via Dockerfile you will need to rebuild the docker image. +You can do it like this: + +``` +./build_docker_humble.sh +``` + ### Optionally Rebuild and run a smacc state machine inside the container Notice that ue_editor_rcl containers are run mapping the current smacc2 source folder so it is possible to do a mixed development between the host (to develop from vscode) and the container (to compile and run the state machines). @@ -105,3 +126,6 @@ ros2 launch sm_dancebot_ue sm_dancebot_ue_launch.py ``` Enjoy experimenting with Unreal Engine in your Docker environment! + +### Connecting the container to VPN +Prototype already done -> todo DOC From 73de18eb18f2115f4ba71ec04087bd36fc9b5927 Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Wed, 28 Jun 2023 21:22:01 +0000 Subject: [PATCH 21/70] changes --- .../sm_dancebot_ue/docker/Dockerfile | 5 ++ .../sm_dancebot_ue/docker/README.md | 76 ++++++++++++++----- .../sm_dancebot_ue/docker/run_editor_smacc.sh | 1 + 3 files changed, 63 insertions(+), 19 deletions(-) diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile b/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile index 5df310cd1..8cbaaaf81 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile @@ -63,6 +63,10 @@ RUN sudo apt install -y code git-lfs # or code-insiders RUN git clone --recurse-submodules https://github.com/rapyuta-robotics/turtlebot3-UE.git -b jammy WORKDIR "/home/$USER/src/turtlebot3-UE" +RUN git checkout abdf483ce14f477d8f255fe19f3527975b3a9dbb +RUN cd Plugins/rclUE && git checkout bae993fa +RUN cd Plugins/RapyutaSimulationPlugins && git checkout bfac015 + RUN git-lfs pull && git submodule foreach git-lfs pull ENV UE5_DIR=/home/$USER/src/UE5.1/UnrealEngine RUN ls && echo ${UE5_DIR} @@ -113,4 +117,5 @@ CMD openvpn --daemon --config /home/$USER/ovpnconfig/ovpnconfig.ovpn --auth-user WORKDIR "/home/$USER/src/turtlebot3-UE" ENV LD_LIBRARY_PATH=/home/ros2_ws/src/turtlebot3-UE/Plugins/rclUE/ThirdParty/ros/lib/:/usr/local/cuda/lib64:/usr/lib/x86_64-linux-gnu/gazebo-11/plugins:/opt/ros/humble/opt/rviz_ogre_vendor/lib:/opt/ros/humble/lib/x86_64-linux-gnu: + #ENTRYPOINT /home/$USER/src/turtlebot3-UE/run_editor_smacc.sh diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md b/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md index 20b58b016..7171bf4d9 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md @@ -45,41 +45,58 @@ Load the downloaded image into your Docker image database using the following co sudo docker load -i ue_editor_rclue.tar ``` -### Running/Creating a new container from the ue_editor_rcl docker image -To run the Docker image, first, download the current SMACC2 repository. Then, navigate to the Docker folder and execute the following command: +### (Alternative) Building docker image locally + +In the case you want to change the docker image via Dockerfile you will need to rebuild the docker image. +Before doing that you need to make a link to the UE5.1 folder into sm_dancebot_ue/docker folder. ``` -./run_docker_container_bash.sh +mount --bind UE5.1/UnrealEngine ``` -This will create and start a new container as a daemon (should be available on restarting). This container is able to open de unreal engine editor with ROS2, but the editor will not open until you run the following command: +Then you can build the image locally. You can do that like this: ``` -./join_editor.sh +./build_docker_humble.sh ``` -The Unreal Engine editor will automatically open in "edition mode." By clicking the "play" button, you can launch the simulation with Turtlebot topics accessible from both the container and the host computer. +## Using the docker Image + +### Running/Creating a new container from the ue_editor_rcl docker image + +To run the unreal editor inside the container we will need some auxiliar scripts that are located in the sm_dancebot_ue example, first, download the current SMACC2 repository. Then, navigate to the `sm_dancebot_ue/docker` folder and execute the following command: +``` +./run_docker_container_editor.sh +``` +The Unreal Engine editor will automatically open in "edition mode." By clicking the "play" button, you can launch the simulation with Turtlebot topics accessible from both the container and the host computer. -### Start/stop an already existing container from the ue_editor_rcl docker image +When you close the editor, the container also will finish, but the container "keeps installed", you can reopen again the editor using the command: ``` ./start_container.sh ``` +### (Alternative) Running/Creating a new container for container debugging + +There is a secondary way of creating the container in a daemon mode so that the lifetime of the container is not tied to the unreal editor window. This is useful specially for developing new features for the container. To create the container in this mode you have to run: + ``` -./stop_container.sh +./run_docker_container_bash.sh ``` -### Join to the container via bash -You may need to enter into the docker container to debug, or test stuff from commandline. -Then you can use the following command: + +This will create and start a new container as a daemon (should be available on restarting). This container is able to open de unreal engine editor with ROS2, but the editor will not open until you run the following command: ``` -./join_bash.sh +./join_editor.sh ``` -### Optionally Stopping and removing running container + +The Unreal Engine editor will automatically open in "edition mode." By clicking the "play" button, you can launch the simulation with Turtlebot topics accessible from both the container and the host computer. + + +### Stopping and removing running container You may want to reset everyting if you messed the container. Before you create a new container you need to stop and remove the existing one. @@ -89,14 +106,37 @@ to stop and remove the existing one. ./remove_container.sh ``` -### Optionally Stopping and removing running container -In the case you want to change the docker image via Dockerfile you will need to rebuild the docker image. -You can do it like this: +### Join to the container via bash +You may need to enter into the docker container to debug, or test stuff from commandline. +Then you can use the following command: ``` -./build_docker_humble.sh +./join_bash.sh +``` + + +## Connecting the container to VPN +Prototype already done -> todo DOC + + +### Important notes about the solution + +*Repos versioning* +Not all the versions of rclUE turtlebot3Editor and RapyputaPlugins are compatible to each other. +Keeping them consistent is not always easy, specially taking into account that the container maps/volumes some volumes with external contents. + +The container is already designed to have a correct combination of all of them, however this is hardcoded in the Dockerfile and it could be improved. + +*DDS Configuration* +We noticed problems communicating the docker container nodes with host nodes. At the current moment the workaround we have for a correct +communication is using in the host cyclonedds (Until we find the causes and the solution to have everything uniform using fastrpts). +To do that, add to the .bashrc file in the host the following line: + ``` +export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp +``` + ### Optionally Rebuild and run a smacc state machine inside the container @@ -127,5 +167,3 @@ ros2 launch sm_dancebot_ue sm_dancebot_ue_launch.py Enjoy experimenting with Unreal Engine in your Docker environment! -### Connecting the container to VPN -Prototype already done -> todo DOC diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_editor_smacc.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_editor_smacc.sh index 53745811c..eac103adf 100755 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_editor_smacc.sh +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_editor_smacc.sh @@ -2,6 +2,7 @@ # Copyright 2020-2022 Rapyuta Robotics Co., Ltd. export LD_LIBRARY_PATH=$(echo "$LD_LIBRARY_PATH" | sed 's#/opt/ros/humble/lib:##') +echo "UPDATED LD_LIBRARY_PATH: $LD_LIBRARY_PATH" if [ -z "${UE5_DIR}" ]; then printf "Please set UE5_DIR to path of UE5 folder\n" From 0946d16d955f425ed3fb33b3384674b92b066cae Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Wed, 28 Jun 2023 21:26:22 +0000 Subject: [PATCH 22/70] progress --- .../sm_dancebot_ue/docker/README.md | 194 ++++++++++++++++++ 1 file changed, 194 insertions(+) diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md b/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md index 7171bf4d9..9f003f6bc 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md @@ -167,3 +167,197 @@ ros2 launch sm_dancebot_ue sm_dancebot_ue_launch.py Enjoy experimenting with Unreal Engine in your Docker environment! +## Pre-Requisites +Before proceeding with the instructions, ensure that you have the necessary prerequisites installed on your system. + +### Docker Installation +To install Docker, follow these steps: + +1. Update the package list: + ``` + sudo apt-get update + ``` + +2. Install the required dependencies: + ``` + sudo apt-get install apt-transport-https ca-certificates curl gnupg-agent software-properties-common + ``` + +3. Add Docker's official GPG key: + ``` + curl -fsSL https://download.docker.com/linux/ubuntu/gpg | sudo apt-key add - + ``` + +4. Add the Docker repository: + ``` + sudo add-apt-repository "deb [arch=amd64] https://download.docker.com/linux/ubuntu bionic stable" + ``` + +5. Update the package list again: + ``` + sudo apt-get update + ``` + +6. Install Docker: + ``` + sudo apt-get install docker-ce + ``` + +### NVIDIA-Docker2 Installation +Ensure that you have NVIDIA-Docker2 installed by executing the following commands: + +1. Add the NVIDIA-Docker GPG key: + ``` + curl -s -L https://nvidia.github.io/nvidia-docker/gpgkey | sudo apt-key add - + ``` + +2. Determine the distribution: + ``` + distribution=$(. /etc/os-release; echo $ID$VERSION_ID) + ``` + +3. Add the NVIDIA-Docker repository: + ``` + curl -s -L https://nvidia.github.io/nvidia-docker/$distribution/nvidia-docker.list | sudo tee /etc/apt/sources.list.d/nvidia-docker.list + ``` + +4. Update the package list: + ``` + sudo apt-get update + ``` + +5. Install NVIDIA-Docker2: + ``` + sudo apt-get install nvidia-docker2 + ``` + +6. Restart the Docker service: + ``` + sudo systemctl restart docker.service + ``` + +### Adding User to Docker Group +To allow a user to run Docker, add the user to the Docker group. Replace `` with the actual user ID: + +``` +sudo usermod -a -G docker +``` + +### Testing the Installation +To test your Docker and NVIDIA-Docker2 installations, run the following command: + +``` +sudo nvidia-docker run --rm nvidia/cuda:10.2-base-ubuntu18.04 nvidia-smi +``` + +## Unreal Engine Docker Image +This section provides instructions for downloading, loading, and running the prebuilt Docker image containing Unreal Engine. + +### Downloading the Prebuilt Image +Download the prebuilt Docker image for Unreal Engine from the provided source. + +### Loading the Docker Image +To load the downloaded image into your Docker image database, use the following command: + +``` +sudo docker load -i ue_editor_rclue.tar +``` + +### (Alternative) Building the Docker Image Locally +If you want to modify the Docker image using a Dockerfile and rebuild it, follow these steps: + +1. Create a link to the UE5.1 folder inside the `sm_dancebot_ue/docker` folder: + ``` + mount --bind UE5.1/UnrealEngine + ``` + +2. Build the image locally by running the following command: + ``` + ./build_docker_humble.sh + ``` + +## Using the Docker Image +This section explains how to run and create a new container from the `ue_editor_rcl` Docker image. + +### Running/Creating a New Container from the ue_editor_rcl Docker Image + +To run the Unreal Editor inside the container, you need to use some auxiliary scripts located in the `sm_dancebot_ue` example. Follow these steps: + +1. Download the current SMACC2 repository. + +2. Navigate to the `sm_dancebot_ue/docker` folder. + +3. Execute the following command: + ``` + ./run_docker_container_editor.sh + ``` + + This will run and create a new container using the `ue_editor_rcl` Docker image. + +4. The Unreal Engine editor will automatically open in "edition mode." You can launch the simulation with Turtlebot topics accessible from both the container and the host computer by clicking the "play" button. + + Note: When you close the editor, the container will also be finished, but it will remain installed. You can reopen the editor using the command: + ``` + ./start_container.sh + ``` + +### (Alternative) Running/Creating a New Container for Container Debugging + +There is an alternative way to create the container in daemon mode, where the lifetime of the container is not tied to the Unreal Editor window. This is useful, especially for developing new features for the container. To create the container in this mode, follow these steps: + +1. Execute the following command: + ``` + ./run_docker_container_bash.sh + ``` + + This will create and start a new container as a daemon. The container will be available even after restarting. It is capable of opening the Unreal Engine editor with ROS2, but the editor will not open until you run the following command: + ``` + ./join_editor.sh + ``` + +2. The Unreal Engine editor will automatically open in "edition mode." You can launch the simulation with Turtlebot topics accessible from both the container and the host computer by clicking the "play" button. + +### Stopping and Removing a Running Container + +If you need to reset everything and remove the existing container, follow these steps: + +1. Execute the following command: + ``` + ./stop_container.sh + ``` + + This will stop the running container. + +2. Execute the following command: + ``` + ./remove_container.sh + ``` + + This will remove the existing container. + +### Joining the Container via Bash + +To enter the Docker container and debug or test things from the command line, use the following command: + +``` +./join_bash.sh +``` + +## Connecting the Container to VPN + +The prototype for connecting the container to a VPN has already been completed. For detailed instructions, please refer to the relevant documentation. + +### Important Notes about the Solution + +Here are some important notes regarding the solution: + +**Repos Versioning:** Not all versions of `rclUE`, `turtlebot3Editor`, and `RapyputaPlugins` are compatible with each other. Ensuring consistency among them can be challenging, especially when considering that the container maps/volumes some volumes with external contents. The container is already designed to have a correct combination of all of them, but this is hardcoded in the Dockerfile and can be improved. + +**DDS Configuration:** There may be communication issues between the Docker container nodes and host nodes. As a workaround, we currently use `cyclonedds` on the host (until a uniform solution using `fastrtps` is found). To set this up, add the following line to the `.bashrc` file on the host: +``` +export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp +``` + +### Optionally Rebuilding and Running a SMACC State Machine inside the Container + +Note that the `ue_editor_rcl` containers are run by mapping the current `smacc2` source folder, enabling mixed development between the host (using \ No newline at end of file From 06b46f6bdf370c24ba2fa0fbe6ba015584a7b7ee Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pablo=20I=C3=B1igo=20Blasco?= Date: Wed, 28 Jun 2023 23:29:47 +0200 Subject: [PATCH 23/70] Update README.md --- .../sm_dancebot_ue/docker/README.md | 171 +----------------- 1 file changed, 1 insertion(+), 170 deletions(-) diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md b/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md index 9f003f6bc..c47f81990 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md @@ -1,172 +1,3 @@ -## Pre Reqs -From https://www.ibm.com/docs/en/maximo-vi/8.2.0?topic=planning-installing-docker-nvidia-docker2 - -Make sure you have Docker installed... -``` -sudo apt-get update -sudo apt-get install apt-transport-https ca-certificates curl gnupg-agent software-properties-common -curl -fsSL https://download.docker.com/linux/ubuntu/gpg | sudo apt-key add - -sudo add-apt-repository "deb [arch=amd64] https://download.docker.com/linux/ubuntu bionic stable" -sudo apt-get update -sudo apt-get install docker-ce -``` -Make sure you have nvidia-docker2 installed - -``` -curl -s -L https://nvidia.github.io/nvidia-docker/gpgkey | sudo apt-key add - -distribution=$(. /etc/os-release;echo $ID$VERSION_ID) -curl -s -L https://nvidia.github.io/nvidia-docker/$distribution/nvidia-docker.list | sudo tee /etc/apt/sources.list.d/nvidia-docker.list -sudo apt-get update -sudo apt-get install nvidia-docker2 -sudo systemctl restart docker.service -``` -For each userid that will run Docker®, add the userid to the docker group: -``` -sudo usermod -a -G docker -``` -Then test.. -``` -sudo nvidia-docker run --rm nvidia/cuda:10.2-base-ubuntu18.04 nvidia-smi -``` -## Unreal Engine Docker Image - -This repository provides a prebuilt Docker image containing Unreal Engine. Follow the instructions below to download, load, and run the image. - -### Downloading the Prebuilt Image - -Download the prebuilt Docker image for Unreal Engine: - - -### Loading the Docker Image - -Load the downloaded image into your Docker image database using the following command: - -``` -sudo docker load -i ue_editor_rclue.tar -``` - - -### (Alternative) Building docker image locally - -In the case you want to change the docker image via Dockerfile you will need to rebuild the docker image. -Before doing that you need to make a link to the UE5.1 folder into sm_dancebot_ue/docker folder. - -``` -mount --bind UE5.1/UnrealEngine -``` - -Then you can build the image locally. You can do that like this: - -``` -./build_docker_humble.sh -``` - -## Using the docker Image - -### Running/Creating a new container from the ue_editor_rcl docker image - -To run the unreal editor inside the container we will need some auxiliar scripts that are located in the sm_dancebot_ue example, first, download the current SMACC2 repository. Then, navigate to the `sm_dancebot_ue/docker` folder and execute the following command: - -``` -./run_docker_container_editor.sh -``` - -The Unreal Engine editor will automatically open in "edition mode." By clicking the "play" button, you can launch the simulation with Turtlebot topics accessible from both the container and the host computer. - -When you close the editor, the container also will finish, but the container "keeps installed", you can reopen again the editor using the command: - -``` -./start_container.sh -``` - -### (Alternative) Running/Creating a new container for container debugging - -There is a secondary way of creating the container in a daemon mode so that the lifetime of the container is not tied to the unreal editor window. This is useful specially for developing new features for the container. To create the container in this mode you have to run: - -``` -./run_docker_container_bash.sh -``` - -This will create and start a new container as a daemon (should be available on restarting). This container is able to open de unreal engine editor with ROS2, but the editor will not open until you run the following command: - -``` -./join_editor.sh -``` - -The Unreal Engine editor will automatically open in "edition mode." By clicking the "play" button, you can launch the simulation with Turtlebot topics accessible from both the container and the host computer. - - -### Stopping and removing running container - -You may want to reset everyting if you messed the container. Before you create a new container you need -to stop and remove the existing one. - -``` -./stop_container.sh -./remove_container.sh -``` - - -### Join to the container via bash -You may need to enter into the docker container to debug, or test stuff from commandline. -Then you can use the following command: - -``` -./join_bash.sh -``` - - -## Connecting the container to VPN -Prototype already done -> todo DOC - - -### Important notes about the solution - -*Repos versioning* -Not all the versions of rclUE turtlebot3Editor and RapyputaPlugins are compatible to each other. -Keeping them consistent is not always easy, specially taking into account that the container maps/volumes some volumes with external contents. - -The container is already designed to have a correct combination of all of them, however this is hardcoded in the Dockerfile and it could be improved. - -*DDS Configuration* -We noticed problems communicating the docker container nodes with host nodes. At the current moment the workaround we have for a correct -communication is using in the host cyclonedds (Until we find the causes and the solution to have everything uniform using fastrpts). -To do that, add to the .bashrc file in the host the following line: - -``` -export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp -``` - - -### Optionally Rebuild and run a smacc state machine inside the container - -Notice that ue_editor_rcl containers are run mapping the current smacc2 source folder so it is possible to do a mixed development between the host (to develop from vscode) and the container (to compile and run the state machines). - -Smacc2 source code is already prebuilt inside the image consecuently it is available to any container. -You could need to modify smacc2 code or examples and rebuilt it. - -If that is the case you can do the following: - -``` -cd ~/ -``` -``` -source /opt/ros/humble/setup.bash -``` -``` -colcon build -``` -To run your current demo, follow these steps after executing `colcon build`: - -``` -source install/setup.bash -``` -``` -ros2 launch sm_dancebot_ue sm_dancebot_ue_launch.py -``` - -Enjoy experimenting with Unreal Engine in your Docker environment! - ## Pre-Requisites Before proceeding with the instructions, ensure that you have the necessary prerequisites installed on your system. @@ -360,4 +191,4 @@ export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp ### Optionally Rebuilding and Running a SMACC State Machine inside the Container -Note that the `ue_editor_rcl` containers are run by mapping the current `smacc2` source folder, enabling mixed development between the host (using \ No newline at end of file +Note that the `ue_editor_rcl` containers are run by mapping the current `smacc2` source folder, enabling mixed development between the host (using From 83ceaf7d7b3407995ad0ea323e8af01dddda0d86 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pablo=20I=C3=B1igo=20Blasco?= Date: Wed, 28 Jun 2023 23:35:28 +0200 Subject: [PATCH 24/70] Update README.md --- .../sm_dancebot_ue/docker/README.md | 22 ++++++++++++------- 1 file changed, 14 insertions(+), 8 deletions(-) diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md b/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md index c47f81990..54ca154a5 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md @@ -134,19 +134,21 @@ To run the Unreal Editor inside the container, you need to use some auxiliary sc ### (Alternative) Running/Creating a New Container for Container Debugging -There is an alternative way to create the container in daemon mode, where the lifetime of the container is not tied to the Unreal Editor window. This is useful, especially for developing new features for the container. To create the container in this mode, follow these steps: +There is an alternative way to create the container in daemon mode, where the lifetime of the container is not tied to the Unreal Editor window. This is useful, especially for developing new features for the container. To create the container in this mode,: 1. Execute the following command: ``` ./run_docker_container_bash.sh ``` - This will create and start a new container as a daemon. The container will be available even after restarting. It is capable of opening the Unreal Engine editor with ROS2, but the editor will not open until you run the following command: + This will create and start a new container as a daemon. The container will be available even after restarting. It is capable of opening the Unreal Engine editor with ROS2, but the editor will not open automatically. + +2. Execute the editor proccess: ``` ./join_editor.sh ``` -2. The Unreal Engine editor will automatically open in "edition mode." You can launch the simulation with Turtlebot topics accessible from both the container and the host computer by clicking the "play" button. +The Unreal Engine editor will automatically open in "edition mode." You can launch the simulation with Turtlebot topics accessible from both the container and the host computer by clicking the "play" button. ### Stopping and Removing a Running Container @@ -174,11 +176,7 @@ To enter the Docker container and debug or test things from the command line, us ./join_bash.sh ``` -## Connecting the Container to VPN - -The prototype for connecting the container to a VPN has already been completed. For detailed instructions, please refer to the relevant documentation. - -### Important Notes about the Solution +## Important Notes about the Solution Here are some important notes regarding the solution: @@ -192,3 +190,11 @@ export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp ### Optionally Rebuilding and Running a SMACC State Machine inside the Container Note that the `ue_editor_rcl` containers are run by mapping the current `smacc2` source folder, enabling mixed development between the host (using + +### Automatic driver update +The host and the container must have the same nvidia-driver in order to run the ue editor and simulation. +There is a mechanism implemented to automatically sync the driver. The current driver version is passed from the host to the container and then it is updated in the container if that is required. That is done in the nvidia-check.sh script. + +### Connecting the Container to VPN + +The prototype for connecting the container to a VPN has already been completed. For detailed instructions, please refer to the relevant documentation. From c9abc9eb51e4c18535d650f04a47659e021d56cf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pablo=20I=C3=B1igo=20Blasco?= Date: Wed, 28 Jun 2023 23:39:21 +0200 Subject: [PATCH 25/70] Update README.md --- .../sm_dancebot_ue/docker/README.md | 56 ++++++++++++++----- 1 file changed, 42 insertions(+), 14 deletions(-) diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md b/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md index 54ca154a5..5beed112d 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md @@ -1,3 +1,21 @@ +# SMACC2 and UNREAL ENGINE EDITOR +The README.md file provides a comprehensive guide for setting up and utilizing the Unreal Engine Docker environment. The document covers the necessary prerequisites, such as installing Docker and NVIDIA-Docker2, and provides step-by-step instructions for downloading and loading the prebuilt Docker image containing Unreal Engine. Additionally, it explains how to run the Unreal Editor within the container, create new containers for debugging purposes, and connect the container to a VPN if required. The README.md also includes important notes and optional instructions for rebuilding and running SMACC state machines inside the container. Whether you are new to Docker or an experienced user, this document will help you navigate the process of working with Unreal Engine in a Docker environment effectively. + +## Important Notes about the Solution + +Here are some important notes regarding the solution: + +**Repos Versioning:** Not all versions of `rclUE`, `turtlebot3Editor`, and `RapyputaPlugins` are compatible with each other. Ensuring consistency among them can be challenging, especially when considering that the container maps/volumes some volumes with external contents. The container is already designed to have a correct combination of all of them, but this is hardcoded in the Dockerfile and can be improved. + +**DDS Configuration:** There may be communication issues between the Docker container nodes and host nodes. As a workaround, we currently use `cyclonedds` on the host (until a uniform solution using `fastrtps` is found). To set this up, add the following line to the `.bashrc` file on the host: +``` +export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp +``` + +***Automatic container nvidia driver update*** +The host and the container must have the same nvidia-driver in order to run the ue editor and simulation. +There is a mechanism implemented to automatically sync the driver. The current driver version is passed from the host to the container and then it is updated in the container if that is required. That is done in the nvidia-check.sh script. + ## Pre-Requisites Before proceeding with the instructions, ensure that you have the necessary prerequisites installed on your system. @@ -176,25 +194,35 @@ To enter the Docker container and debug or test things from the command line, us ./join_bash.sh ``` -## Important Notes about the Solution +### Connecting the Container to VPN -Here are some important notes regarding the solution: +The prototype for connecting the container to a VPN has already been completed. For detailed instructions, please refer to the relevant documentation. -**Repos Versioning:** Not all versions of `rclUE`, `turtlebot3Editor`, and `RapyputaPlugins` are compatible with each other. Ensuring consistency among them can be challenging, especially when considering that the container maps/volumes some volumes with external contents. The container is already designed to have a correct combination of all of them, but this is hardcoded in the Dockerfile and can be improved. +### Optionally Rebuilding and Running a SMACC State Machine inside the Container -**DDS Configuration:** There may be communication issues between the Docker container nodes and host nodes. As a workaround, we currently use `cyclonedds` on the host (until a uniform solution using `fastrtps` is found). To set this up, add the following line to the `.bashrc` file on the host: -``` -export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp -``` +Note that the `ue_editor_rcl` containers are run by mapping the current `smacc2` source folder, allowing for mixed development between the host (using VSCode) and the container (for compiling and running the state machines). -### Optionally Rebuilding and Running a SMACC State Machine inside the Container +The SMACC2 source code is already prebuilt inside the image, making it available to any container. However, if you need to modify the `smacc2` code or examples and rebuild it, follow these steps: -Note that the `ue_editor_rcl` containers are run by mapping the current `smacc2` source folder, enabling mixed development between the host (using +1. Change to the home directory: + ``` + cd ~/ + ``` -### Automatic driver update -The host and the container must have the same nvidia-driver in order to run the ue editor and simulation. -There is a mechanism implemented to automatically sync the driver. The current driver version is passed from the host to the container and then it is updated in the container if that is required. That is done in the nvidia-check.sh script. +2. Source the Humble ROS 2 installation: + ``` + source /opt/ros/humble/setup.bash + ``` -### Connecting the Container to VPN +3. Build the `smacc2` code: + ``` + colcon build + ``` -The prototype for connecting the container to a VPN has already been completed. For detailed instructions, please refer to the relevant documentation. +4. To run your current demo, execute the following commands after building: + ``` + source install/setup.bash + ros2 launch sm_dancebot_ue sm_dancebot_ue_launch.py + ``` + +Enjoy experimenting with Unreal Engine in your Docker environment! From 185e82205f100e98d69825666912b6633fe00160 Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Fri, 30 Jun 2023 07:39:39 +0000 Subject: [PATCH 26/70] updates in navigation --- .../launch/sm_dancebot_ue_launch.py | 7 ++ .../params/nav2z_client/nav2_params.yaml | 29 ++++- .../scripts/test_oscillation.py | 110 ++++++++++++------ .../urdf/turtlebot3_waffle.urdf | 4 +- 4 files changed, 112 insertions(+), 38 deletions(-) diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/launch/sm_dancebot_ue_launch.py b/smacc2_sm_reference_library/sm_dancebot_ue/launch/sm_dancebot_ue_launch.py index 1f896fa13..86e9700d7 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/launch/sm_dancebot_ue_launch.py +++ b/smacc2_sm_reference_library/sm_dancebot_ue/launch/sm_dancebot_ue_launch.py @@ -143,6 +143,13 @@ def generate_launch_description(): arguments=[urdf], ) + # static_transform_publisher = Node( + # package="tf2_ros", + # executable="static_transform_publisher", + # name="static_transform_publisher", + # output="screen", + # arguments=["0", "0", "0", "0", "0", "0", "basel_link", "odom"], + rviz_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join(sm_dance_bot_launch_dir, "rviz_launch.py")), condition=IfCondition(use_rviz), diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/nav2_params.yaml b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/nav2_params.yaml index 3b3987700..ecf88b01a 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/nav2_params.yaml +++ b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/nav2_params.yaml @@ -268,13 +268,16 @@ global_costmap: update_frequency: 2.0 publish_frequency: 1.0 transform_tolerance: 5.0 - global_frame: map + global_frame: odom robot_base_frame: base_link use_sim_time: True + rolling_window: true robot_radius: 0.22 - resolution: 0.05 + resolution: 0.1 + width: 8 + height: 8 track_unknown_space: true - plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + plugins: ["voxel_layer", "inflation_layer"] #"static_layer" obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True @@ -289,6 +292,26 @@ global_costmap: raytrace_min_range: 0.0 obstacle_max_range: 15.5 obstacle_min_range: 0.0 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 15.0 + raytrace_min_range: 0.0 + obstacle_max_range: 15.5 + obstacle_min_range: 0.0 static_layer: plugin: "nav2_costmap_2d::StaticLayer" map_subscribe_transient_local: True diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/scripts/test_oscillation.py b/smacc2_sm_reference_library/sm_dancebot_ue/scripts/test_oscillation.py index 2e9af4887..a4f547bf0 100755 --- a/smacc2_sm_reference_library/sm_dancebot_ue/scripts/test_oscillation.py +++ b/smacc2_sm_reference_library/sm_dancebot_ue/scripts/test_oscillation.py @@ -1,3 +1,4 @@ +#!/usr/bin/env python3 import rclpy from rclpy.node import Node from geometry_msgs.msg import Twist @@ -5,101 +6,144 @@ from tf2_ros import Buffer from tf2_ros import TransformStamped import math +import tf_transformations +import time +import std_msgs.msg -class RotationOscillation(Node): +class RotationOscillation(Node): def __init__(self): - super().__init__('rotation_oscillation') - self.cmd_vel_pub = self.create_publisher(Twist, 'cmd_vel', 10) + super().__init__("rotation_oscillation") + self.cmd_vel_pub = self.create_publisher(Twist, "cmd_vel", 1) + self.yaw_pub = self.create_publisher(std_msgs.msg.Float64, "yaw", 1) + self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer, self) + self.on_timer = self.create_timer(0.05, self.on_timer_callback) + + self.base_link_frame = "base_footprint" + self.fixed_frame = "odom" + + def on_timer_callback(self): + try: + base_link_transform = self.tf_buffer.lookup_transform( + self.fixed_frame, self.base_link_frame, rclpy.time.Time() + ) + q = base_link_transform.transform.rotation + euler = tf_transformations.euler_from_quaternion([q.x, q.y, q.z, q.w]) + self.get_logger().info("Current rotation: %f" % math.degrees(euler[2])) + self.get_logger().info("Transform available!") + + t = float(self.get_clock().now().nanoseconds) / 1e9 + cmd_vel = Twist() + cmd_vel.angular.z = 1.0 if math.sin(t) > 0.0 else -1.0 + self.cmd_vel_pub.publish(cmd_vel) + + self.yaw_pub.publish(std_msgs.msg.Float64(data=euler[2])) + + self.get_logger().info("Current time: %s" % str(t)) + except Exception as e: + self.get_logger().error("Error getting transform: %s" % str(e)) def oscillate_rotation(self): rate = self.create_rate(1) angle = math.radians(25) - timeout = rclpy.duration.Duration(seconds=1.0) # Timeout for waiting on transforms - base_link_frame = 'base_footprint' - fixed_frame = 'odom' + timeout = rclpy.duration.Duration(seconds=1.0) # Timeout for waiting on transforms - print('Waiting for transform from "fixed_frame" to "base_link_frame"...') - while not self.tf_buffer.can_transform(base_link_frame, fixed_frame, rclpy.time.Time(), timeout): - self.get_logger().info('Waiting for transform from "fixed_frame" to "base_link_frame"...') + print(f'Waiting for transform from "{fixed_frame}" to "{base_link_frame}"...') + while rclpy.ok(): + # self.get_logger().info(f'Waiting for transform from "{fixed_frame}" to "{base_link_frame}"...') rclpy.spin_once(self) + # self.get_logger().info('Spin once, now: %s' % str(self.get_clock().now().toSec())) rate.sleep() - self.get_logger().info('Transform available!') + self.get_logger().info("Transform available!") # Get initial position - print('Getting initial position...') + print("Getting initial position...") try: - base_link_transform = self.tf_buffer.lookup_transform('map', base_link_frame, rclpy.time.Time()) + base_link_transform = self.tf_buffer.lookup_transform( + "map", base_link_frame, rclpy.time.Time() + ) initial_position = base_link_transform.transform.translation except Exception as e: - self.get_logger().error('Error getting initial position: %s' % str(e)) + self.get_logger().error("Error getting initial position: %s" % str(e)) return # Rotate 25 degrees to the left - print('Rotating 25 degrees to the left...') - self.get_logger().info('Rotating 25 degrees to the left...') + print("Rotating 25 degrees to the left...") + self.get_logger().info("Rotating 25 degrees to the left...") twist_msg = Twist() twist_msg.angular.z = angle self.cmd_vel_pub.publish(twist_msg) rclpy.spin_once(self) # Get position after left rotation - print('Getting position after left rotation...') + print("Getting position after left rotation...") try: - print('Getting position after left rotation...') - base_link_transform = self.tf_buffer.lookup_transform('map', base_link_frame, rclpy.time.Time()) + print("Getting position after left rotation...") + base_link_transform = self.tf_buffer.lookup_transform( + "map", base_link_frame, rclpy.time.Time() + ) left_rotation_position = base_link_transform.transform.translation except Exception as e: - self.get_logger().error('Error getting position after left rotation: %s' % str(e)) + self.get_logger().error("Error getting position after left rotation: %s" % str(e)) return # Rotate 25 degrees to the right - print('Rotating 25 degrees to the right...') - self.get_logger().info('Rotating 25 degrees to the right...') + print("Rotating 25 degrees to the right...") + self.get_logger().info("Rotating 25 degrees to the right...") twist_msg.angular.z = -angle self.cmd_vel_pub.publish(twist_msg) rclpy.spin_once(self) # Get position after right rotation try: - base_link_transform = self.tf_buffer.lookup_transform('map', base_link_frame, rclpy.time.Time()) + base_link_transform = self.tf_buffer.lookup_transform( + "map", base_link_frame, rclpy.time.Time() + ) right_rotation_position = base_link_transform.transform.translation except Exception as e: - self.get_logger().error('Error getting position after right rotation: %s' % str(e)) + self.get_logger().error("Error getting position after right rotation: %s" % str(e)) return # Calculate phase difference - print('Calculating phase difference...') - left_rotation_angle = math.atan2(left_rotation_position.y - initial_position.y, - left_rotation_position.x - initial_position.x) - right_rotation_angle = math.atan2(right_rotation_position.y - initial_position.y, - right_rotation_position.x - initial_position.x) + print("Calculating phase difference...") + left_rotation_angle = math.atan2( + left_rotation_position.y - initial_position.y, + left_rotation_position.x - initial_position.x, + ) + right_rotation_angle = math.atan2( + right_rotation_position.y - initial_position.y, + right_rotation_position.x - initial_position.x, + ) phase_difference = math.degrees(right_rotation_angle - left_rotation_angle) - self.get_logger().info('Phase difference: %f degrees' % phase_difference) + self.get_logger().info("Phase difference: %f degrees" % phase_difference) # Stop the robot twist_msg.angular.z = 0.0 self.cmd_vel_pub.publish(twist_msg) def shutdown(self): - self.get_logger().info('Shutting down...') + self.get_logger().info("Shutting down...") self.destroy_node() + def main(args=None): rclpy.init(args=args) rotation_oscillation = RotationOscillation() + rclpy.spin(rotation_oscillation) + try: rotation_oscillation.oscillate_rotation() except Exception as e: - rotation_oscillation.get_logger().error('Error during rotation oscillation: %s' % str(e)) + rotation_oscillation.get_logger().error("Error during rotation oscillation: %s" % str(e)) rotation_oscillation.shutdown() rclpy.shutdown() -if __name__ == '__main__': - main() \ No newline at end of file + +if __name__ == "__main__": + main() diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/urdf/turtlebot3_waffle.urdf b/smacc2_sm_reference_library/sm_dancebot_ue/urdf/turtlebot3_waffle.urdf index c92483449..f4307672c 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/urdf/turtlebot3_waffle.urdf +++ b/smacc2_sm_reference_library/sm_dancebot_ue/urdf/turtlebot3_waffle.urdf @@ -106,7 +106,7 @@ - + @@ -138,7 +138,7 @@ - + From a80731d0cf6ac04f057e4c159bfdce48a954f56b Mon Sep 17 00:00:00 2001 From: brettpac Date: Fri, 30 Jun 2023 12:34:04 -0700 Subject: [PATCH 27/70] Bretts runtime instructions added to readme --- .../sm_dancebot_ue/docker/README.md | 67 +++++++++++++++++++ 1 file changed, 67 insertions(+) diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md b/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md index 5beed112d..2974eb08b 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md @@ -226,3 +226,70 @@ The SMACC2 source code is already prebuilt inside the image, making it available ``` Enjoy experimenting with Unreal Engine in your Docker environment! + + + +# Brett's runtime notes + +You'll need to open three terminals for this demo. + One for the container where you'll run Unreal Engine + One to run the state machine (on the host) + One for the RTA (on the host) + +# Terminal 1 - For UE5 Simulation in the container + +1. Download the current SMACC2 repository. + ``` + cd ~/workspace/humble_ws/src + git clone https://github.com/robosoft-ai/SMACC2.git + + ``` +2. Build the workspace + + ``` + cd ~/workspace/humble_ws/ + source /opt/ros/humble/setup.bash + colcon build + + ``` + Once everything is done building... +3. Navigate to the `sm_dancebot_ue/docker` folder. + + ``` + cd ~/workspace/humble_ws/src/SMACC2/smacc2_sm_reference_library/sm_dancebot_ue/docker + ``` +4. Execute the following command: + ``` + ./run_docker_container_editor.sh + ``` + This will create and start a new container as a daemon. The container will be available even after restarting. It is capable of opening the Unreal Engine editor with ROS2, but the editor will not open automatically. + The Unreal Engine editor will automatically open in "edition mode." You can launch the simulation with Turtlebot topics accessible from both the container and the host computer by clicking the "play" button. + +# Terminal 2 - for the state machine on the host + +1. Add the following line to the `.bashrc` file on the host: + ``` + export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp + ``` +2. Source the workspace you just built + ``` + source ~/workspace/humble_ws/install/setup.bash + ``` +3. Launch the state machine + ``` + ros2 launch sm_dancebot_ue sm_dancebot_ue_launch.py + ``` + This will launch the state machine application, rviz and other required nodes. + +# Terminal 3 - for the SMACC2_RTA on the host +1. Source the install + ``` + source /opt/ros/humble/setup.bash + ``` +2. Launch the SMACC2_RTA + ``` + ros2 run smacc2_rta smacc2_rta + ``` +3. Once the RTA is launched, in the upper left corner select State Machine/Available State Machines/SmDanceBotUE + +And you should be all set. \ No newline at end of file From e94cf8614959feb443357d599c1bc2a1bff026fe Mon Sep 17 00:00:00 2001 From: brettpac Date: Fri, 30 Jun 2023 12:37:45 -0700 Subject: [PATCH 28/70] minor change --- smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md b/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md index 2974eb08b..b46551146 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md @@ -256,7 +256,7 @@ You'll need to open three terminals for this demo. 3. Navigate to the `sm_dancebot_ue/docker` folder. ``` - cd ~/workspace/humble_ws/src/SMACC2/smacc2_sm_reference_library/sm_dancebot_ue/docker + cd ~/workspace/humble_ws/src/SMACC2/smacc2_sm_reference_library/sm_dancebot_ue/docker ``` 4. Execute the following command: ``` From 3a5ac4b62e7e590860dccae3690871183b17e796 Mon Sep 17 00:00:00 2001 From: brettpac Date: Fri, 30 Jun 2023 12:40:13 -0700 Subject: [PATCH 29/70] Heading sizes in readme --- smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md b/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md index b46551146..a3c948496 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md @@ -236,7 +236,7 @@ You'll need to open three terminals for this demo. One to run the state machine (on the host) One for the RTA (on the host) -# Terminal 1 - For UE5 Simulation in the container +### Terminal 1 - For UE5 Simulation in the container 1. Download the current SMACC2 repository. ``` @@ -265,7 +265,7 @@ You'll need to open three terminals for this demo. This will create and start a new container as a daemon. The container will be available even after restarting. It is capable of opening the Unreal Engine editor with ROS2, but the editor will not open automatically. The Unreal Engine editor will automatically open in "edition mode." You can launch the simulation with Turtlebot topics accessible from both the container and the host computer by clicking the "play" button. -# Terminal 2 - for the state machine on the host +### Terminal 2 - for the state machine on the host 1. Add the following line to the `.bashrc` file on the host: ``` @@ -281,7 +281,7 @@ You'll need to open three terminals for this demo. ``` This will launch the state machine application, rviz and other required nodes. -# Terminal 3 - for the SMACC2_RTA on the host +### Terminal 3 - for the SMACC2_RTA on the host 1. Source the install ``` source /opt/ros/humble/setup.bash From faea4b3334bac6b2e0c104274effdbe5f47c3c0c Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Fri, 30 Jun 2023 20:27:53 +0000 Subject: [PATCH 30/70] improvements in navigation --- .../sm_dancebot_ue/CMakeLists.txt | 3 +- .../states/st_navigate_to_waypoints_x.hpp | 5 +- .../launch/online_sync_launch.py | 6 +- .../launch/sm_dancebot_ue_launch.py | 36 ++++++++--- .../params/mapper_params_online_sync.yaml | 4 +- .../params/nav2z_client/nav2_params.yaml | 61 +++++++------------ .../params/nav2z_client/waypoints_plan.yaml | 4 +- .../rviz/nav2_default_view.rviz | 35 +++++------ 8 files changed, 80 insertions(+), 74 deletions(-) diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/CMakeLists.txt b/smacc2_sm_reference_library/sm_dancebot_ue/CMakeLists.txt index 25b52357d..4e6a2dad8 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/CMakeLists.txt +++ b/smacc2_sm_reference_library/sm_dancebot_ue/CMakeLists.txt @@ -94,7 +94,8 @@ install(TARGETS install(FILES - servers/service_node_3/service_node_3.py + servers/service_node_3/service_node_3.py + scripts/transform_publisher.py DESTINATION lib/${PROJECT_NAME} PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READ diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_to_waypoints_x.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_to_waypoints_x.hpp index 702e9263d..5062fb6c7 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_to_waypoints_x.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_to_waypoints_x.hpp @@ -40,8 +40,9 @@ struct StNavigateToWaypointsX : smacc2::SmaccState, StNavigateToWaypointsX, TRANSITION_1>, - Transition, SS1::SsRadialPattern1, TRANSITION_2>, - Transition, SS2::SsRadialPattern2, TRANSITION_3> + Transition, StNavigateToWaypointsX, TRANSITION_2>, + Transition, SS1::SsRadialPattern1, TRANSITION_3>, + Transition, SS2::SsRadialPattern2, TRANSITION_4> >reactions; diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/launch/online_sync_launch.py b/smacc2_sm_reference_library/sm_dancebot_ue/launch/online_sync_launch.py index 154a08cda..e8d0b7c7c 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/launch/online_sync_launch.py +++ b/smacc2_sm_reference_library/sm_dancebot_ue/launch/online_sync_launch.py @@ -10,6 +10,7 @@ def generate_launch_description(): use_sim_time = LaunchConfiguration("use_sim_time") slam_params_file = LaunchConfiguration("slam_params_file") + xtermprefix = "xterm -xrm 'XTerm*scrollBar: true' -xrm 'xterm*rightScrollBar: true' -hold -geometry 1000x600 -sl 10000 -e" declare_use_sim_time_argument = DeclareLaunchArgument( "use_sim_time", default_value="true", description="Use simulation/Gazebo clock" @@ -17,7 +18,9 @@ def generate_launch_description(): declare_slam_params_file_cmd = DeclareLaunchArgument( "slam_params_file", default_value=os.path.join( - get_package_share_directory("slam_toolbox"), "config", "mapper_params_online_sync.yaml" + #get_package_share_directory("slam_toolbox"), "config", "mapper_params_online_sync.yaml" + get_package_share_directory("sm_dancebot_ue"), "params", "mapper_params_online_sync.yaml" + ), description="Full path to the ROS2 parameters file to use for the slam_toolbox node", ) @@ -28,6 +31,7 @@ def generate_launch_description(): executable="sync_slam_toolbox_node", name="slam_toolbox", output="screen", + prefix= xtermprefix ) ld = LaunchDescription() diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/launch/sm_dancebot_ue_launch.py b/smacc2_sm_reference_library/sm_dancebot_ue/launch/sm_dancebot_ue_launch.py index 86e9700d7..2d73b980f 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/launch/sm_dancebot_ue_launch.py +++ b/smacc2_sm_reference_library/sm_dancebot_ue/launch/sm_dancebot_ue_launch.py @@ -27,6 +27,9 @@ def generate_launch_description(): + + xtermprefix = "xterm -xrm 'XTerm*scrollBar: true' -xrm 'xterm*rightScrollBar: true' -hold -geometry 1000x600 -sl 10000 -e" + # Get the launch directory sm_dance_bot_dir = get_package_share_directory("sm_dancebot_ue") sm_dance_bot_launch_dir = os.path.join(sm_dance_bot_dir, "launch") @@ -91,6 +94,8 @@ def generate_launch_description(): description="Use headless Gazebo if true", ) + + declare_params_file_cmd = DeclareLaunchArgument( "params_file", default_value=os.path.join(sm_dance_bot_dir, "params", "nav2z_client", "nav2_params.yaml"), @@ -143,12 +148,25 @@ def generate_launch_description(): arguments=[urdf], ) - # static_transform_publisher = Node( - # package="tf2_ros", - # executable="static_transform_publisher", - # name="static_transform_publisher", - # output="screen", - # arguments=["0", "0", "0", "0", "0", "0", "basel_link", "odom"], + static_transform_publisher = Node( + package="sm_dancebot_ue", + executable="transform_publisher.py", + name="static_transform_publisherxx", + output="screen", + #arguments=["-0.064", "0", "0.122", "0", "0", "0", "base_scan", "base_link"], + prefix= xtermprefix, + parameters=[{"use_sim_time": use_sim_time}], + + ) + + static_transform_publisher_2 = Node( + package="tf2_ros", + executable="static_transform_publisher", + name="static_transform_publisher", + output="screen", + arguments=["0", "0", "0", "0", "0", "0", "base_link", "base_footprint"], + prefix= xtermprefix, + ) rviz_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join(sm_dance_bot_launch_dir, "rviz_launch.py")), @@ -174,7 +192,6 @@ def generate_launch_description(): }.items(), ) - xtermprefix = "xterm -xrm 'XTerm*scrollBar: true' -xrm 'xterm*rightScrollBar: true' -hold -geometry 1000x600 -sl 10000 -e" sm_dance_bot_node = Node( package="sm_dancebot_ue", @@ -220,6 +237,8 @@ def generate_launch_description(): ], ) + + # Create the launch description and populate ld = LaunchDescription() @@ -244,6 +263,9 @@ def generate_launch_description(): ld.add_action(service3_node) ld.add_action(temperature_action_server) ld.add_action(led_action_server_node) + ld.add_action(static_transform_publisher) + #ld.add_action(static_transform_publisher_2) + # # Add the actions to launch all of the navigation nodes ld.add_action(start_robot_state_publisher_cmd) diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/params/mapper_params_online_sync.yaml b/smacc2_sm_reference_library/sm_dancebot_ue/params/mapper_params_online_sync.yaml index 01204f181..5b8589169 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/params/mapper_params_online_sync.yaml +++ b/smacc2_sm_reference_library/sm_dancebot_ue/params/mapper_params_online_sync.yaml @@ -26,8 +26,8 @@ slam_toolbox: debug_logging: false throttle_scans: 1 transform_publish_period: 0.02 #if 0 never publishes odometry - map_update_interval: 1.0 - resolution: 0.01 + map_update_interval: 0.05 + resolution: 0.03 max_laser_range: 20.0 #for rastering images minimum_time_interval: 0.5 transform_timeout: 0.2 diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/nav2_params.yaml b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/nav2_params.yaml index ecf88b01a..96bf0f2c5 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/nav2_params.yaml +++ b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/nav2_params.yaml @@ -52,7 +52,7 @@ bt_navigator: use_sim_time: True global_frame: map robot_base_frame: base_link - odom_topic: /odom2 + odom_topic: /odom enable_groot_monitoring: True groot_zmq_publisher_port: 1666 groot_zmq_server_port: 1667 @@ -115,8 +115,8 @@ controller_server: # Goal checker parameters goal_checker: plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 0.457 - yaw_goal_tolerance: 0.265 # almost free orientation + xy_goal_tolerance: 0.257 + yaw_goal_tolerance: 0.165 # almost free orientation stateful: True backward_goal_checker: plugin: "nav2_controller::SimpleGoalChecker" @@ -145,20 +145,20 @@ controller_server: debug_trajectory_details: True min_vel_x: 0.0 min_vel_y: 0.0 - max_vel_x: 0.2 + max_vel_x: 0.3 max_vel_y: 0.0 - max_vel_theta: 0.2 + max_vel_theta: 1.0 min_speed_xy: 0.0 - max_speed_xy: 0.2 + max_speed_xy: 0.3 min_speed_theta: 0.0 # Add high threshold velocity for turtlebot 3 issue. # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 - acc_lim_x: 2.0 + acc_lim_x: 2.5 acc_lim_y: 0.0 - acc_lim_theta: 1.2 + acc_lim_theta: 3.2 decel_lim_x: -2.5 decel_lim_y: 0.0 - decel_lim_theta: -1.2 + decel_lim_theta: -3.2 vx_samples: 20 vy_samples: 5 vtheta_samples: 20 @@ -191,7 +191,7 @@ controller_server: carrot_distance: 0.3 #meters default 0.5 carrot_angular_distance: 0.3 # no constraint, max 3.1416 pure_spinning_straight_line_mode: true - max_linear_x_speed: 0.2 + max_linear_x_speed: 0.25 ForwardLocalPlanner: plugin: "cl_nav2z::forward_local_planner::ForwardLocalPlanner" transform_tolerance: 0.5 @@ -200,7 +200,7 @@ controller_server: k_betta: -1.0 carrot_distance: 0.2 #meters carrot_angular_distance: 0.5 # no constraint, max 3.1416 - max_linear_x_speed: 0.2 + max_linear_x_speed: 0.3 max_angular_z_speed: 0.2 PureSpinningLocalPlanner: plugin: "pure_spinning_local_planner::PureSpinningLocalPlanner" @@ -217,8 +217,8 @@ controller_server_rclcpp_node: local_costmap: local_costmap: ros__parameters: - update_frequency: 10.0 - publish_frequency: 5.0 + update_frequency: 5.0 + publish_frequency: 2.0 global_frame: odom robot_base_frame: base_link use_sim_time: True @@ -230,8 +230,8 @@ local_costmap: plugins: ["voxel_layer", "inflation_layer"] inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 2.0 - inflation_radius: 0.25 + cost_scaling_factor: 3.0 + inflation_radius: 0.55 voxel_layer: plugin: "nav2_costmap_2d::VoxelLayer" enabled: True @@ -268,16 +268,19 @@ global_costmap: update_frequency: 2.0 publish_frequency: 1.0 transform_tolerance: 5.0 - global_frame: odom + width: 16 + height: 16 + global_frame: map robot_base_frame: base_link use_sim_time: True rolling_window: true + robot_radius: 0.22 resolution: 0.1 width: 8 height: 8 track_unknown_space: true - plugins: ["voxel_layer", "inflation_layer"] #"static_layer" + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True @@ -292,33 +295,13 @@ global_costmap: raytrace_min_range: 0.0 obstacle_max_range: 15.5 obstacle_min_range: 0.0 - voxel_layer: - plugin: "nav2_costmap_2d::VoxelLayer" - enabled: True - publish_voxel_map: True - origin_z: 0.0 - z_resolution: 0.05 - z_voxels: 16 - max_obstacle_height: 2.0 - mark_threshold: 0 - observation_sources: scan - scan: - topic: /scan - max_obstacle_height: 2.0 - clearing: True - marking: True - data_type: "LaserScan" - raytrace_max_range: 15.0 - raytrace_min_range: 0.0 - obstacle_max_range: 15.5 - obstacle_min_range: 0.0 static_layer: plugin: "nav2_costmap_2d::StaticLayer" map_subscribe_transient_local: True inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 2.0 - inflation_radius: 0.25 + cost_scaling_factor: 1.0 + inflation_radius: 0.55 always_send_full_costmap: True global_costmap_client: ros__parameters: diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan.yaml b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan.yaml index 04546657e..eed06d5e4 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan.yaml +++ b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan.yaml @@ -1,8 +1,8 @@ waypoints: # POINT 0 - position: - x: 2.0 - y: 2.1 + x: 1.8 + y: 2.8 z: 0.0 orientation: x: 0.0 diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/rviz/nav2_default_view.rviz b/smacc2_sm_reference_library/sm_dancebot_ue/rviz/nav2_default_view.rviz index 6a506a1ab..eaca2d2c2 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/rviz/nav2_default_view.rviz +++ b/smacc2_sm_reference_library/sm_dancebot_ue/rviz/nav2_default_view.rviz @@ -9,9 +9,12 @@ Panels: - /TF1/Frames1 - /TF1/Tree1 - /LaserScan1 - - /BackwardLocalPlan1 + - /Global Planner1 + - /Global Planner1/Global Costmap1 + - /Controller1 + - /Controller1/Local Costmap1 Splitter Ratio: 0.5833333134651184 - Tree Height: 865 + Tree Height: 654 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -104,10 +107,6 @@ Visualization Manager: Value: true odom: Value: true - wheel_left_link: - Value: true - wheel_right_link: - Value: true Marker Scale: 1 Name: TF Show Arrows: false @@ -133,10 +132,6 @@ Visualization Manager: {} imu_link: {} - wheel_left_link: - {} - wheel_right_link: - {} Update Interval: 0 Value: true - Alpha: 1 @@ -251,7 +246,7 @@ Visualization Manager: Value: true - Class: rviz_common/Group Displays: - - Alpha: 0.30000001192092896 + - Alpha: 0.10000000149011612 Class: rviz_default_plugins/Map Color Scheme: costmap Draw Behind: false @@ -351,7 +346,7 @@ Visualization Manager: Name: Global Planner - Class: rviz_common/Group Displays: - - Alpha: 0.699999988079071 + - Alpha: 0.10000000149011612 Class: rviz_default_plugins/Map Color Scheme: costmap Draw Behind: false @@ -660,7 +655,7 @@ Visualization Manager: Enabled: true Name: ForwardCarrot Namespaces: - my_namespace2: true + {} Topic: Depth: 5 Durability Policy: Volatile @@ -773,7 +768,7 @@ Visualization Manager: Value: true Views: Current: - Angle: -4.785000801086426 + Angle: -4.795001029968262 Class: rviz_default_plugins/TopDownOrtho Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 @@ -783,7 +778,7 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Scale: -123.52355194091797 + Scale: -91.58348083496094 Target Frame: base_link Value: TopDownOrtho (rviz_default_plugins) X: -0.6155462265014648 @@ -792,12 +787,12 @@ Visualization Manager: Window Geometry: Displays: collapsed: true - Height: 1016 + Height: 829 Hide Left Dock: true Hide Right Dock: true Navigation 2: collapsed: false - QMainWindow State: 000000ff00000000fd0000000400000000000002860000039efc020000000afb0000001200530065006c0065006300740069006f006e000000003d000000830000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003200000002da000001010000013900fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000002800ffffff000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000007380000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd000000040000000000000286000002d5fc020000000afb0000001200530065006c0065006300740069006f006e000000003d000000830000006e00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730000000047000002d5000000f100fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003200000002da000001010000017100fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000002f00ffffff000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000039e000000c200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000546000002d500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 RealsenseCamera: collapsed: false Selection: @@ -806,6 +801,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1848 - X: 72 - Y: 27 + Width: 1350 + X: 90 + Y: 34 From 3227b5a722709ba11df7abedc5be09fed165cbe1 Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Sat, 1 Jul 2023 11:23:57 +0000 Subject: [PATCH 31/70] fixing build --- .../sm_dance_bot/launch/bringup_launch.py | 4 +- .../sm_dance_bot/launch/gazebo_launch.py | 2 +- .../sm_dancebot_ue/CMakeLists.txt | 2 +- .../sm_dancebot_ue/docker/README.md | 2 +- .../sm_dancebot_ue/launch/bringup_launch.py | 4 +- .../launch/online_sync_launch.py | 26 +++++- .../launch/sm_dancebot_ue_launch.py | 41 ++++------ .../scripts/test_oscillation.py | 22 ++++- .../scripts/transform_publisher.py | 81 +++++++++++++++++++ 9 files changed, 145 insertions(+), 39 deletions(-) create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/scripts/transform_publisher.py diff --git a/smacc2_sm_reference_library/sm_dance_bot/launch/bringup_launch.py b/smacc2_sm_reference_library/sm_dance_bot/launch/bringup_launch.py index 1714986e4..f125a27e2 100644 --- a/smacc2_sm_reference_library/sm_dance_bot/launch/bringup_launch.py +++ b/smacc2_sm_reference_library/sm_dance_bot/launch/bringup_launch.py @@ -25,7 +25,7 @@ ) from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PythonExpression +from launch.substitutions import LaunchConfiguration from launch_ros.actions import PushRosNamespace @@ -38,7 +38,7 @@ def generate_launch_description(): namespace = LaunchConfiguration("namespace") use_namespace = LaunchConfiguration("use_namespace") slam = LaunchConfiguration("slam") - map_yaml_file = LaunchConfiguration("map") + # map_yaml_file = LaunchConfiguration("map") use_sim_time = LaunchConfiguration("use_sim_time") params_file = LaunchConfiguration("params_file") default_nav_to_pose_bt_xml = LaunchConfiguration("default_nav_to_pose_bt_xml") diff --git a/smacc2_sm_reference_library/sm_dance_bot/launch/gazebo_launch.py b/smacc2_sm_reference_library/sm_dance_bot/launch/gazebo_launch.py index 30ead3d33..68fdc89fc 100644 --- a/smacc2_sm_reference_library/sm_dance_bot/launch/gazebo_launch.py +++ b/smacc2_sm_reference_library/sm_dance_bot/launch/gazebo_launch.py @@ -19,7 +19,7 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, ExecuteProcess from launch.conditions import IfCondition, UnlessCondition -from launch.substitutions import LaunchConfiguration, PythonExpression +from launch.substitutions import LaunchConfiguration def generate_launch_description(): diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/CMakeLists.txt b/smacc2_sm_reference_library/sm_dancebot_ue/CMakeLists.txt index 4e6a2dad8..9c4d7eccc 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/CMakeLists.txt +++ b/smacc2_sm_reference_library/sm_dancebot_ue/CMakeLists.txt @@ -94,7 +94,7 @@ install(TARGETS install(FILES - servers/service_node_3/service_node_3.py + servers/service_node_3/service_node_3.py scripts/transform_publisher.py DESTINATION lib/${PROJECT_NAME} diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md b/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md index a3c948496..3973b6c1e 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md @@ -292,4 +292,4 @@ You'll need to open three terminals for this demo. ``` 3. Once the RTA is launched, in the upper left corner select State Machine/Available State Machines/SmDanceBotUE -And you should be all set. \ No newline at end of file +And you should be all set. diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/launch/bringup_launch.py b/smacc2_sm_reference_library/sm_dancebot_ue/launch/bringup_launch.py index b3aa81173..c7ebca467 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/launch/bringup_launch.py +++ b/smacc2_sm_reference_library/sm_dancebot_ue/launch/bringup_launch.py @@ -25,7 +25,7 @@ ) from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PythonExpression +from launch.substitutions import LaunchConfiguration from launch_ros.actions import PushRosNamespace @@ -38,7 +38,7 @@ def generate_launch_description(): namespace = LaunchConfiguration("namespace") use_namespace = LaunchConfiguration("use_namespace") slam = LaunchConfiguration("slam") - map_yaml_file = LaunchConfiguration("map") + # map_yaml_file = LaunchConfiguration("map") use_sim_time = LaunchConfiguration("use_sim_time") params_file = LaunchConfiguration("params_file") default_nav_to_pose_bt_xml = LaunchConfiguration("default_nav_to_pose_bt_xml") diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/launch/online_sync_launch.py b/smacc2_sm_reference_library/sm_dancebot_ue/launch/online_sync_launch.py index e8d0b7c7c..e3487e7a3 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/launch/online_sync_launch.py +++ b/smacc2_sm_reference_library/sm_dancebot_ue/launch/online_sync_launch.py @@ -1,3 +1,20 @@ +# Copyright (c) 2023 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 + + import os from launch import LaunchDescription @@ -18,9 +35,10 @@ def generate_launch_description(): declare_slam_params_file_cmd = DeclareLaunchArgument( "slam_params_file", default_value=os.path.join( - #get_package_share_directory("slam_toolbox"), "config", "mapper_params_online_sync.yaml" - get_package_share_directory("sm_dancebot_ue"), "params", "mapper_params_online_sync.yaml" - + # get_package_share_directory("slam_toolbox"), "config", "mapper_params_online_sync.yaml" + get_package_share_directory("sm_dancebot_ue"), + "params", + "mapper_params_online_sync.yaml", ), description="Full path to the ROS2 parameters file to use for the slam_toolbox node", ) @@ -31,7 +49,7 @@ def generate_launch_description(): executable="sync_slam_toolbox_node", name="slam_toolbox", output="screen", - prefix= xtermprefix + prefix=xtermprefix, ) ld = LaunchDescription() diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/launch/sm_dancebot_ue_launch.py b/smacc2_sm_reference_library/sm_dancebot_ue/launch/sm_dancebot_ue_launch.py index 2d73b980f..6ea966123 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/launch/sm_dancebot_ue_launch.py +++ b/smacc2_sm_reference_library/sm_dancebot_ue/launch/sm_dancebot_ue_launch.py @@ -40,12 +40,10 @@ def generate_launch_description(): use_namespace = LaunchConfiguration("use_namespace") map_yaml_file = LaunchConfiguration("map") use_sim_time = LaunchConfiguration("use_sim_time") - gazebo_headless = LaunchConfiguration("headless") params_file = LaunchConfiguration("params_file") default_nav_to_pose_bt_xml = LaunchConfiguration("default_nav_to_pose_bt_xml") autostart = LaunchConfiguration("autostart") - show_gz_lidar = LaunchConfiguration("show_gz_lidar") # Launch configuration variables specific to simulation rviz_config_file = LaunchConfiguration("rviz_config_file") @@ -94,8 +92,6 @@ def generate_launch_description(): description="Use headless Gazebo if true", ) - - declare_params_file_cmd = DeclareLaunchArgument( "params_file", default_value=os.path.join(sm_dance_bot_dir, "params", "nav2z_client", "nav2_params.yaml"), @@ -149,24 +145,23 @@ def generate_launch_description(): ) static_transform_publisher = Node( - package="sm_dancebot_ue", - executable="transform_publisher.py", - name="static_transform_publisherxx", - output="screen", - #arguments=["-0.064", "0", "0.122", "0", "0", "0", "base_scan", "base_link"], - prefix= xtermprefix, - parameters=[{"use_sim_time": use_sim_time}], - + package="sm_dancebot_ue", + executable="transform_publisher.py", + name="static_transform_publisherxx", + output="screen", + # arguments=["-0.064", "0", "0.122", "0", "0", "0", "base_scan", "base_link"], + prefix=xtermprefix, + parameters=[{"use_sim_time": use_sim_time}], ) - static_transform_publisher_2 = Node( - package="tf2_ros", - executable="static_transform_publisher", - name="static_transform_publisher", - output="screen", - arguments=["0", "0", "0", "0", "0", "0", "base_link", "base_footprint"], - prefix= xtermprefix, - ) + # static_transform_publisher_2 = Node( + # package="tf2_ros", + # executable="static_transform_publisher", + # name="static_transform_publisher", + # output="screen", + # arguments=["0", "0", "0", "0", "0", "0", "base_link", "base_footprint"], + # prefix=xtermprefix, + # ) rviz_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join(sm_dance_bot_launch_dir, "rviz_launch.py")), @@ -192,7 +187,6 @@ def generate_launch_description(): }.items(), ) - sm_dance_bot_node = Node( package="sm_dancebot_ue", executable="sm_dancebot_ue_node", @@ -237,8 +231,6 @@ def generate_launch_description(): ], ) - - # Create the launch description and populate ld = LaunchDescription() @@ -264,8 +256,7 @@ def generate_launch_description(): ld.add_action(temperature_action_server) ld.add_action(led_action_server_node) ld.add_action(static_transform_publisher) - #ld.add_action(static_transform_publisher_2) - + # ld.add_action(static_transform_publisher_2) # # Add the actions to launch all of the navigation nodes ld.add_action(start_robot_state_publisher_cmd) diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/scripts/test_oscillation.py b/smacc2_sm_reference_library/sm_dancebot_ue/scripts/test_oscillation.py index a4f547bf0..3008e133b 100755 --- a/smacc2_sm_reference_library/sm_dancebot_ue/scripts/test_oscillation.py +++ b/smacc2_sm_reference_library/sm_dancebot_ue/scripts/test_oscillation.py @@ -1,13 +1,26 @@ #!/usr/bin/env python3 + +# 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 rclpy from rclpy.node import Node from geometry_msgs.msg import Twist from tf2_ros import TransformListener from tf2_ros import Buffer -from tf2_ros import TransformStamped import math import tf_transformations -import time import std_msgs.msg @@ -48,7 +61,10 @@ def on_timer_callback(self): def oscillate_rotation(self): rate = self.create_rate(1) angle = math.radians(25) - timeout = rclpy.duration.Duration(seconds=1.0) # Timeout for waiting on transforms + # timeout = rclpy.duration.Duration(seconds=1.0) # Timeout for waiting on transforms + + fixed_frame = "odom" + base_link_frame = "base_footprint" print(f'Waiting for transform from "{fixed_frame}" to "{base_link_frame}"...') while rclpy.ok(): diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/scripts/transform_publisher.py b/smacc2_sm_reference_library/sm_dancebot_ue/scripts/transform_publisher.py new file mode 100644 index 000000000..58c5e4df7 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/scripts/transform_publisher.py @@ -0,0 +1,81 @@ +#!/usr/bin/env python3 + +# 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 rclpy +from rclpy.node import Node +import tf2_ros +from geometry_msgs.msg import TransformStamped + + +class StaticTransformPublisher(Node): + def __init__(self, name, parent_frame, child_frame, xyz=[0.0, 0.0, 0.0], rpy=[0.0, 0.0, 0.0]): + super().__init__(name) + self.transform_broadcaster = tf2_ros.TransformBroadcaster(self) + + self.timer = self.create_timer(0.05, self.publish_transform) + self.transform = TransformStamped() + self.transform.header.frame_id = parent_frame + self.transform.child_frame_id = child_frame + self.transform.transform.translation.x = xyz[0] + self.transform.transform.translation.y = xyz[1] + self.transform.transform.translation.z = xyz[2] + + + self.transform.transform.rotation.x = 0.0 + self.transform.transform.rotation.y = 0.0 + self.transform.transform.rotation.z = 0.0 + self.transform.transform.rotation.w = 1.0 + + def publish_transform(self): + self.transform.header.stamp = self.get_clock().now().to_msg() + self.transform_broadcaster.sendTransform(self.transform) + + +def main(args=None): + rclpy.init(args=args) + + # Create the first instance of StaticTransformPublisher + node1 = StaticTransformPublisher( + 'static_transform_publisher_1', + 'base_footprint', + 'base_link', + ) + + # Create the second instance of StaticTransformPublisher + node2 = StaticTransformPublisher( + 'static_transform_publisher_2', + 'base_link', + 'base_scan', + xyz=[-0.064, 0.0, 0.122], + + ) + + try: + # Spin both nodes simultaneously + while rclpy.ok(): + rclpy.spin_once(node1) + rclpy.spin_once(node2) + except KeyboardInterrupt: + pass + + node1.destroy_node() + node2.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() + From 22dce8a8b7fd677531bc31c229ff4196c4cf745a Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Sat, 1 Jul 2023 11:38:25 +0000 Subject: [PATCH 32/70] fixing format --- .../scripts/transform_publisher.py | 17 +++++++---------- 1 file changed, 7 insertions(+), 10 deletions(-) diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/scripts/transform_publisher.py b/smacc2_sm_reference_library/sm_dancebot_ue/scripts/transform_publisher.py index 58c5e4df7..a1692716d 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/scripts/transform_publisher.py +++ b/smacc2_sm_reference_library/sm_dancebot_ue/scripts/transform_publisher.py @@ -33,7 +33,6 @@ def __init__(self, name, parent_frame, child_frame, xyz=[0.0, 0.0, 0.0], rpy=[0. self.transform.transform.translation.y = xyz[1] self.transform.transform.translation.z = xyz[2] - self.transform.transform.rotation.x = 0.0 self.transform.transform.rotation.y = 0.0 self.transform.transform.rotation.z = 0.0 @@ -49,18 +48,17 @@ def main(args=None): # Create the first instance of StaticTransformPublisher node1 = StaticTransformPublisher( - 'static_transform_publisher_1', - 'base_footprint', - 'base_link', + "static_transform_publisher_1", + "base_footprint", + "base_link", ) # Create the second instance of StaticTransformPublisher node2 = StaticTransformPublisher( - 'static_transform_publisher_2', - 'base_link', - 'base_scan', + "static_transform_publisher_2", + "base_link", + "base_scan", xyz=[-0.064, 0.0, 0.122], - ) try: @@ -76,6 +74,5 @@ def main(args=None): rclpy.shutdown() -if __name__ == '__main__': +if __name__ == "__main__": main() - From 1eeb044c6fde22bd04972308f81df497ab6d07b3 Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Sat, 1 Jul 2023 11:47:42 +0000 Subject: [PATCH 33/70] fixing branch --- smacc2_sm_reference_library/sm_dance_bot/launch/gazebo_launch.py | 1 + 1 file changed, 1 insertion(+) diff --git a/smacc2_sm_reference_library/sm_dance_bot/launch/gazebo_launch.py b/smacc2_sm_reference_library/sm_dance_bot/launch/gazebo_launch.py index 32ad55976..68fdc89fc 100644 --- a/smacc2_sm_reference_library/sm_dance_bot/launch/gazebo_launch.py +++ b/smacc2_sm_reference_library/sm_dance_bot/launch/gazebo_launch.py @@ -21,6 +21,7 @@ from launch.conditions import IfCondition, UnlessCondition from launch.substitutions import LaunchConfiguration + def generate_launch_description(): declare_use_simulator_cmd = DeclareLaunchArgument( "use_simulator", default_value="false", description="Whether to execute gzclient)" From 4a4140e3aaa329acdc847bfbc5076037e8f0500e Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Sat, 1 Jul 2023 15:56:16 +0000 Subject: [PATCH 34/70] progress --- .../sm_dancebot_ue/docker/Dockerfile | 2 ++ .../sm_dancebot_ue/params/mapper_params_online_sync.yaml | 8 ++++---- .../params/nav2z_client/waypoints_plan.yaml | 4 ++-- .../sm_dancebot_ue/scripts/transform_publisher.py | 4 ++-- 4 files changed, 10 insertions(+), 8 deletions(-) diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile b/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile index 8cbaaaf81..23ff1ed20 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile @@ -65,6 +65,8 @@ RUN git clone --recurse-submodules https://github.com/rapyuta-robotics/turtlebot WORKDIR "/home/$USER/src/turtlebot3-UE" RUN git checkout abdf483ce14f477d8f255fe19f3527975b3a9dbb RUN cd Plugins/rclUE && git checkout bae993fa + +RUN rm -rf Plugins/RapyutaSimulationPlugins && git clone https://github.com/robosoft-ai/UE-Plugins.git Plugins/RapyutaSimulationPlugins RUN cd Plugins/RapyutaSimulationPlugins && git checkout bfac015 RUN git-lfs pull && git submodule foreach git-lfs pull diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/params/mapper_params_online_sync.yaml b/smacc2_sm_reference_library/sm_dancebot_ue/params/mapper_params_online_sync.yaml index 5b8589169..0f948602c 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/params/mapper_params_online_sync.yaml +++ b/smacc2_sm_reference_library/sm_dancebot_ue/params/mapper_params_online_sync.yaml @@ -23,13 +23,13 @@ slam_toolbox: #map_start_pose: [0.0, 0.0, 0.0] #map_start_at_dock: true - debug_logging: false + debug_logging: true throttle_scans: 1 transform_publish_period: 0.02 #if 0 never publishes odometry map_update_interval: 0.05 - resolution: 0.03 - max_laser_range: 20.0 #for rastering images - minimum_time_interval: 0.5 + resolution: 0.04 + max_laser_range: 16.0 #for rastering images + minimum_time_interval: 0.3 transform_timeout: 0.2 tf_buffer_duration: 30. stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan.yaml b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan.yaml index eed06d5e4..36a8206b8 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan.yaml +++ b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan.yaml @@ -1,8 +1,8 @@ waypoints: # POINT 0 - position: - x: 1.8 - y: 2.8 + x: 1.5 + y: 2.5 z: 0.0 orientation: x: 0.0 diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/scripts/transform_publisher.py b/smacc2_sm_reference_library/sm_dancebot_ue/scripts/transform_publisher.py index a1692716d..622200fe4 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/scripts/transform_publisher.py +++ b/smacc2_sm_reference_library/sm_dancebot_ue/scripts/transform_publisher.py @@ -25,7 +25,7 @@ def __init__(self, name, parent_frame, child_frame, xyz=[0.0, 0.0, 0.0], rpy=[0. super().__init__(name) self.transform_broadcaster = tf2_ros.TransformBroadcaster(self) - self.timer = self.create_timer(0.05, self.publish_transform) + self.timer = self.create_timer(0.01, self.publish_transform) self.transform = TransformStamped() self.transform.header.frame_id = parent_frame self.transform.child_frame_id = child_frame @@ -39,7 +39,7 @@ def __init__(self, name, parent_frame, child_frame, xyz=[0.0, 0.0, 0.0], rpy=[0. self.transform.transform.rotation.w = 1.0 def publish_transform(self): - self.transform.header.stamp = self.get_clock().now().to_msg() + self.transform.header.stamp = rclpy.time.Time(0) self.transform_broadcaster.sendTransform(self.transform) From e5869f4c57d5f7801eff7c8d3adefb71c4c93d39 Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Sat, 1 Jul 2023 16:00:23 +0000 Subject: [PATCH 35/70] repos --- smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile b/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile index 23ff1ed20..fb06ec470 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile @@ -61,7 +61,9 @@ RUN sudo apt install -y apt-transport-https RUN sudo apt update RUN sudo apt install -y code git-lfs # or code-insiders -RUN git clone --recurse-submodules https://github.com/rapyuta-robotics/turtlebot3-UE.git -b jammy +#RUN git clone --recurse-submodules https://github.com/rapyuta-robotics/turtlebot3-UE.git -b jammy +RUN git clone --recurse-submodules https://github.com/robosoft-ai/turtlebot3-UE.git -b jammy + WORKDIR "/home/$USER/src/turtlebot3-UE" RUN git checkout abdf483ce14f477d8f255fe19f3527975b3a9dbb RUN cd Plugins/rclUE && git checkout bae993fa From e91ba60def500d2828ae6519211f45069ca6fab2 Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Sat, 1 Jul 2023 16:05:37 +0000 Subject: [PATCH 36/70] Dockerfile referencing our work --- smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile b/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile index fb06ec470..53093460f 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile @@ -62,7 +62,7 @@ RUN sudo apt update RUN sudo apt install -y code git-lfs # or code-insiders #RUN git clone --recurse-submodules https://github.com/rapyuta-robotics/turtlebot3-UE.git -b jammy -RUN git clone --recurse-submodules https://github.com/robosoft-ai/turtlebot3-UE.git -b jammy +RUN git clone --recurse-submodules https://github.com/robosoft-ai/turtlebot3-UE.git -b feature/smacc2_tests WORKDIR "/home/$USER/src/turtlebot3-UE" RUN git checkout abdf483ce14f477d8f255fe19f3527975b3a9dbb From 80422b63e97db5685a7b042e6743ebc9651e2114 Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Thu, 6 Jul 2023 19:20:15 +0000 Subject: [PATCH 37/70] progress on docker image for ue --- .../sm_dancebot_ue/docker/Dockerfile | 26 +++++++++---------- .../sm_dancebot_ue/docker/run_editor_smacc.sh | 4 +-- 2 files changed, 15 insertions(+), 15 deletions(-) diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile b/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile index 53093460f..8ae8f3baf 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile @@ -62,14 +62,14 @@ RUN sudo apt update RUN sudo apt install -y code git-lfs # or code-insiders #RUN git clone --recurse-submodules https://github.com/rapyuta-robotics/turtlebot3-UE.git -b jammy -RUN git clone --recurse-submodules https://github.com/robosoft-ai/turtlebot3-UE.git -b feature/smacc2_tests +RUN git clone --recurse-submodules https://github.com/robosoft-ai/ue_project_1.git -b smacc2_devel -WORKDIR "/home/$USER/src/turtlebot3-UE" -RUN git checkout abdf483ce14f477d8f255fe19f3527975b3a9dbb -RUN cd Plugins/rclUE && git checkout bae993fa +WORKDIR "/home/$USER/src/ue_project_1" +# RUN git checkout abdf483ce14f477d8f255fe19f3527975b3a9dbb +# RUN cd Plugins/rclUE && git checkout bae993fa -RUN rm -rf Plugins/RapyutaSimulationPlugins && git clone https://github.com/robosoft-ai/UE-Plugins.git Plugins/RapyutaSimulationPlugins -RUN cd Plugins/RapyutaSimulationPlugins && git checkout bfac015 +#RUN rm -rf Plugins/RapyutaSimulationPlugins && git clone https://github.com/robosoft-ai/UE-Plugins.git Plugins/RapyutaSimulationPlugins +#RUN cd Plugins/RapyutaSimulationPlugins && git checkout bfac015 RUN git-lfs pull && git submodule foreach git-lfs pull ENV UE5_DIR=/home/$USER/src/UE5.1/UnrealEngine @@ -82,7 +82,7 @@ RUN wget http://security.ubuntu.com/ubuntu/pool/universe/t/tinyxml2/libtinyxml2- RUN wget http://archive.ubuntu.com/ubuntu/pool/main/o/openssl/libssl1.1_1.1.0g-2ubuntu4_amd64.deb RUN sudo dpkg -i libtinyxml2-6a_7.0.0+dfsg-1build1_amd64.deb RUN sudo dpkg -i libssl1.1_1.1.0g-2ubuntu4_amd64.deb -ENV LD_LIBRARY_PATH=/usr/local/cuda/lib64:/usr/lib/x86_64-linux-gnu/gazebo-11/plugins:/opt/ros/humble/opt/rviz_ogre_vendor/lib:/opt/ros/humble/lib/x86_64-linux-gnu:/home/$USER/src/turtlebot3-UE/Plugins/rclUE/ThirdParty/ros/lib/ +ENV LD_LIBRARY_PATH=/usr/local/cuda/lib64:/usr/lib/x86_64-linux-gnu/gazebo-11/plugins:/opt/ros/humble/opt/rviz_ogre_vendor/lib:/opt/ros/humble/lib/x86_64-linux-gnu:/home/$USER/src/ue_project_1/Plugins/rclUE/ThirdParty/ros/lib/ ENV UE5_DIR=/home/$USER/src/UE5.1/UnrealEngine RUN make turtlebot3 @@ -107,10 +107,10 @@ RUN rosdep install --ignore-src --from-paths . -y -r RUN sudo apt-get update && sudo apt-get install -y ros-humble-nav2-bringup ros-humble-rviz2 RUN sudo apt-get install -y openvpn -COPY --chown=$USER:$USER smacc2_sm_reference_library/sm_dancebot_ue/docker/run_editor_smacc.sh /home/$USER/src/turtlebot3-UE/run_editor_smacc.sh -COPY --chown=$USER:$USER smacc2_sm_reference_library/sm_dancebot_ue/docker/nvidia-driver-check.sh /home/$USER/src/turtlebot3-UE/nvidia-driver-check.sh +COPY --chown=$USER:$USER smacc2_sm_reference_library/sm_dancebot_ue/docker/run_editor_smacc.sh /home/$USER/src/ue_project_1/run_editor_smacc.sh +COPY --chown=$USER:$USER smacc2_sm_reference_library/sm_dancebot_ue/docker/nvidia-driver-check.sh /home/$USER/src/ue_project_1/nvidia-driver-check.sh -RUN touch /home/$USER/src/turtlebot3-UE/COLCON_IGNORE +RUN touch /home/$USER/src/ue_project_1/COLCON_IGNORE RUN touch /home/$USER/src/UE5.1/COLCON_IGNORE WORKDIR "/home/$USER/" RUN bash -c "source /opt/ros/humble/setup.bash && colcon build --symlink-install" @@ -118,8 +118,8 @@ RUN bash -c "source /opt/ros/humble/setup.bash && colcon build --symlink-install RUN mkdir /home/$USER/ovpnconfig CMD openvpn --daemon --config /home/$USER/ovpnconfig/ovpnconfig.ovpn --auth-user-pass /home/$USER/ovpnconfig/login.txt --askpass /home/$USER/ovpnconfig/password.conf -WORKDIR "/home/$USER/src/turtlebot3-UE" +WORKDIR "/home/$USER/src/ue_project_1" -ENV LD_LIBRARY_PATH=/home/ros2_ws/src/turtlebot3-UE/Plugins/rclUE/ThirdParty/ros/lib/:/usr/local/cuda/lib64:/usr/lib/x86_64-linux-gnu/gazebo-11/plugins:/opt/ros/humble/opt/rviz_ogre_vendor/lib:/opt/ros/humble/lib/x86_64-linux-gnu: +ENV LD_LIBRARY_PATH=/home/ros2_ws/src/ue_project_1/Plugins/rclUE/ThirdParty/ros/lib/:/usr/local/cuda/lib64:/usr/lib/x86_64-linux-gnu/gazebo-11/plugins:/opt/ros/humble/opt/rviz_ogre_vendor/lib:/opt/ros/humble/lib/x86_64-linux-gnu: -#ENTRYPOINT /home/$USER/src/turtlebot3-UE/run_editor_smacc.sh +#ENTRYPOINT /home/$USER/src/ue_project_1/run_editor_smacc.sh diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_editor_smacc.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_editor_smacc.sh index eac103adf..7c4723e76 100755 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_editor_smacc.sh +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_editor_smacc.sh @@ -21,7 +21,7 @@ TB3_UE_DIR=${2:-"${CURRENT_DIR}"} # fi #change default level, generating DefautlEngine.ini -DEFAULT_LEVEL=${LEVEL_NAME:-"Turtlebot3_benchmark"} +DEFAULT_LEVEL=${LEVEL_NAME:-"LargeGround"} DEFAULT_RATE=${FIXED_FRAME_RATE:-"100.0"} DEFAULT_RTF=${TARGET_RTF:-"1.0"} sed -e 's/${LEVEL_NAME}/'${DEFAULT_LEVEL}'/g' Config/DefaultEngineBase.ini > Config/DefaultEngine.ini @@ -29,4 +29,4 @@ sed -i -e 's/${FIXED_FRAME_RATE}/'${DEFAULT_RATE}'/g' Config/DefaultEngine.ini sed -i -e 's/${TARGET_RTF}/'${DEFAULT_RTF}'/g' Config/DefaultEngine.ini UE_EDITOR="${UE5_DIR}/Engine/Binaries/Linux/UnrealEditor" -(exec "$UE_EDITOR" "${TB3_UE_DIR}/turtlebot3.uproject" -norelativemousemode) +(exec "$UE_EDITOR" "${TB3_UE_DIR}/ue_project1.uproject" -norelativemousemode) \ No newline at end of file From 2535dac13a71932c70b20a4eb0abbd335cbfcaac Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Thu, 6 Jul 2023 19:22:13 +0000 Subject: [PATCH 38/70] missing --- .../sm_dancebot_ue/docker/run_editor_smacc.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_editor_smacc.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_editor_smacc.sh index 7c4723e76..ee67e2ef4 100755 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_editor_smacc.sh +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_editor_smacc.sh @@ -29,4 +29,4 @@ sed -i -e 's/${FIXED_FRAME_RATE}/'${DEFAULT_RATE}'/g' Config/DefaultEngine.ini sed -i -e 's/${TARGET_RTF}/'${DEFAULT_RTF}'/g' Config/DefaultEngine.ini UE_EDITOR="${UE5_DIR}/Engine/Binaries/Linux/UnrealEditor" -(exec "$UE_EDITOR" "${TB3_UE_DIR}/ue_project1.uproject" -norelativemousemode) \ No newline at end of file +(exec "$UE_EDITOR" "${TB3_UE_DIR}/ue_project1.uproject" -norelativemousemode) From a55802de73213c3c5d90720dc7b575e9eac9e3c0 Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Thu, 6 Jul 2023 21:04:47 +0000 Subject: [PATCH 39/70] changes in the code --- .../sm_dancebot_ue/docker/run_editor_smacc.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_editor_smacc.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_editor_smacc.sh index ee67e2ef4..aa13d0389 100755 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_editor_smacc.sh +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_editor_smacc.sh @@ -21,7 +21,7 @@ TB3_UE_DIR=${2:-"${CURRENT_DIR}"} # fi #change default level, generating DefautlEngine.ini -DEFAULT_LEVEL=${LEVEL_NAME:-"LargeGround"} +DEFAULT_LEVEL=${LEVEL_NAME:-"Default"} DEFAULT_RATE=${FIXED_FRAME_RATE:-"100.0"} DEFAULT_RTF=${TARGET_RTF:-"1.0"} sed -e 's/${LEVEL_NAME}/'${DEFAULT_LEVEL}'/g' Config/DefaultEngineBase.ini > Config/DefaultEngine.ini From 0bdbdc4965d2891a17447b6705770c7e39729f39 Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Thu, 6 Jul 2023 21:12:51 +0000 Subject: [PATCH 40/70] missing stuff --- .../sm_dancebot_ue/docker/Dockerfile | 6 +++--- .../sm_dancebot_ue/docker/build_docker.sh | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile b/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile index 8ae8f3baf..75ea0f54b 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile @@ -1,6 +1,6 @@ -ARG ROS_DISTRO=rolling -ARG GIT_BRANCH=master -ARG UBUNTU_VERSION=focal +ARG ROS_DISTRO +ARG GIT_BRANCH +ARG UBUNTU_VERSION FROM ros:$ROS_DISTRO-ros-base-$UBUNTU_VERSION diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/build_docker.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/build_docker.sh index 518fcd307..da6efd900 100755 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/build_docker.sh +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/build_docker.sh @@ -18,4 +18,4 @@ echo "ubuntu version: $UBUNTU_VERSION" echo "root path: $ROOT_DIR" cd $ROOT_DIR -sudo docker build --build-arg ROS_DISTRO=$ROS_DISTRO --build-arg GIT_BRANCH=$GIT_BRANCH --build-arg UBUNTU_VERSION=$UBUNTU_VERSION -t ue_editor_rclue:$ROS_DISTRO --progress string -f $ROOT_DIR/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile . $NOCACHE +sudo docker build --build-arg ROS_DISTRO=$ROS_DISTRO --build-arg GIT_BRANCH=$GIT_BRANCH --build-arg UBUNTU_VERSION=$UBUNTU_VERSION -t ue_editor_rclue:$ROS_DISTRO -f $ROOT_DIR/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile . $NOCACHE From 3c0b0b230c6dc7e55b836d65c02b96948a24f71c Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Mon, 10 Jul 2023 18:09:20 +0000 Subject: [PATCH 41/70] dockerfile and launching --- .../sm_dancebot_ue/docker/Dockerfile | 33 +++++++++++-------- .../docker/run_docker_container_bash.sh | 2 +- .../run_docker_container_bash_development.sh | 8 +++++ .../docker/run_docker_container_editor.sh | 2 +- 4 files changed, 30 insertions(+), 15 deletions(-) create mode 100755 smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_bash_development.sh diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile b/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile index 75ea0f54b..78078fef0 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile @@ -52,6 +52,10 @@ RUN echo "copying current code version to docker image:" COPY --chown=$USER:$USER smacc2_sm_reference_library/sm_dancebot_ue/docker/UE5.1/UnrealEngine /home/$USER/src/UE5.1/UnrealEngine +ENV UE5_DIR=/home/$USER/src/UE5.1/UnrealEngine +ENV PROJECT_BRANCH=smacc2_devel_stable +ENV PROJECT_NAME=ue_project_1 + RUN sudo apt-get install -y wget gpg RUN wget -qO- https://packages.microsoft.com/keys/microsoft.asc | sudo gpg --dearmor > packages.microsoft.gpg RUN sudo install -D -o root -g root -m 644 packages.microsoft.gpg /etc/apt/keyrings/packages.microsoft.gpg @@ -61,10 +65,10 @@ RUN sudo apt install -y apt-transport-https RUN sudo apt update RUN sudo apt install -y code git-lfs # or code-insiders -#RUN git clone --recurse-submodules https://github.com/rapyuta-robotics/turtlebot3-UE.git -b jammy -RUN git clone --recurse-submodules https://github.com/robosoft-ai/ue_project_1.git -b smacc2_devel +# smacc2_devel +RUN git clone --recurse-submodules https://github.com/robosoft-ai/${PROJECT_NAME}.git -b $PROJECT_BRANCH -WORKDIR "/home/$USER/src/ue_project_1" +WORKDIR "/home/$USER/src/${PROJECT_NAME}" # RUN git checkout abdf483ce14f477d8f255fe19f3527975b3a9dbb # RUN cd Plugins/rclUE && git checkout bae993fa @@ -72,7 +76,6 @@ WORKDIR "/home/$USER/src/ue_project_1" #RUN cd Plugins/RapyutaSimulationPlugins && git checkout bfac015 RUN git-lfs pull && git submodule foreach git-lfs pull -ENV UE5_DIR=/home/$USER/src/UE5.1/UnrealEngine RUN ls && echo ${UE5_DIR} RUN ./update_project_files.sh ENV UE5_DIR=/home/$USER/src/UE5.1 @@ -82,13 +85,16 @@ RUN wget http://security.ubuntu.com/ubuntu/pool/universe/t/tinyxml2/libtinyxml2- RUN wget http://archive.ubuntu.com/ubuntu/pool/main/o/openssl/libssl1.1_1.1.0g-2ubuntu4_amd64.deb RUN sudo dpkg -i libtinyxml2-6a_7.0.0+dfsg-1build1_amd64.deb RUN sudo dpkg -i libssl1.1_1.1.0g-2ubuntu4_amd64.deb -ENV LD_LIBRARY_PATH=/usr/local/cuda/lib64:/usr/lib/x86_64-linux-gnu/gazebo-11/plugins:/opt/ros/humble/opt/rviz_ogre_vendor/lib:/opt/ros/humble/lib/x86_64-linux-gnu:/home/$USER/src/ue_project_1/Plugins/rclUE/ThirdParty/ros/lib/ + +ENV LD_LIBRARY_PATH=/usr/local/cuda/lib64:/usr/lib/x86_64-linux-gnu/gazebo-11/plugins:/opt/ros/humble/opt/rviz_ogre_vendor/lib:/opt/ros/humble/lib/x86_64-linux-gnu:/home/$USER/src/${PROJECT_NAME}/Plugins/rclUE/ThirdParty/ros/lib/ ENV UE5_DIR=/home/$USER/src/UE5.1/UnrealEngine -RUN make turtlebot3 -RUN make turtlebot3Editor +COPY --chown=$USER:$USER smacc2_sm_reference_library/sm_dancebot_ue/docker/nvidia-driver-check.sh /home/$USER/src/${PROJECT_NAME}/nvidia-driver-check.sh +RUN cat Makefile + +RUN make $PROJECT_NAME -# COPY smacc2 /home/$USER/src/SMACC2/smacc2 + # COPY smacc2 /home/$USER/src/SMACC2/smacc2 RUN mkdir -p /home/$USER/src/SMACC2/ WORKDIR "/home/$USER/src/SMACC2" COPY --chown=$USER:$USER smacc2_client_library /home/$USER/src/SMACC2/smacc2_client_library @@ -107,19 +113,20 @@ RUN rosdep install --ignore-src --from-paths . -y -r RUN sudo apt-get update && sudo apt-get install -y ros-humble-nav2-bringup ros-humble-rviz2 RUN sudo apt-get install -y openvpn -COPY --chown=$USER:$USER smacc2_sm_reference_library/sm_dancebot_ue/docker/run_editor_smacc.sh /home/$USER/src/ue_project_1/run_editor_smacc.sh -COPY --chown=$USER:$USER smacc2_sm_reference_library/sm_dancebot_ue/docker/nvidia-driver-check.sh /home/$USER/src/ue_project_1/nvidia-driver-check.sh +#COPY --chown=$USER:$USER smacc2_sm_reference_library/sm_dancebot_ue/docker/run_editor_smacc.sh /home/$USER/src/${PROJECT_NAME}/run_editor_smacc.sh + -RUN touch /home/$USER/src/ue_project_1/COLCON_IGNORE +RUN touch /home/$USER/src/${PROJECT_NAME}/COLCON_IGNORE RUN touch /home/$USER/src/UE5.1/COLCON_IGNORE WORKDIR "/home/$USER/" RUN bash -c "source /opt/ros/humble/setup.bash && colcon build --symlink-install" RUN mkdir /home/$USER/ovpnconfig +# BASIC VPN FRAMEWORK --- CMD openvpn --daemon --config /home/$USER/ovpnconfig/ovpnconfig.ovpn --auth-user-pass /home/$USER/ovpnconfig/login.txt --askpass /home/$USER/ovpnconfig/password.conf -WORKDIR "/home/$USER/src/ue_project_1" +WORKDIR "/home/$USER/src/${PROJECT_NAME}" -ENV LD_LIBRARY_PATH=/home/ros2_ws/src/ue_project_1/Plugins/rclUE/ThirdParty/ros/lib/:/usr/local/cuda/lib64:/usr/lib/x86_64-linux-gnu/gazebo-11/plugins:/opt/ros/humble/opt/rviz_ogre_vendor/lib:/opt/ros/humble/lib/x86_64-linux-gnu: +#ENV LD_LIBRARY_PATH=/home/ros2_ws/src/ue_project_1/Plugins/rclUE/ThirdParty/ros/lib/:/usr/local/cuda/lib64:/usr/lib/x86_64-linux-gnu/gazebo-11/plugins:/opt/ros/humble/opt/rviz_ogre_vendor/lib:/opt/ros/humble/lib/x86_64-linux-gnu: #ENTRYPOINT /home/$USER/src/ue_project_1/run_editor_smacc.sh diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_bash.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_bash.sh index 8effa45bb..5ab294c08 100755 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_bash.sh +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_bash.sh @@ -5,4 +5,4 @@ DIR="$(dirname "$(realpath "$0")")" ROOT_DIR=`realpath $DIR/../../../..` DRIVER=$(apt list --installed | grep nvidia-driver) -sudo nvidia-docker run --net host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name smacc_ue -v $ROOT_DIR/SMACC2:/home/ros2_ws/src/SMACC2 -v $ROOT_DIR/UE-Plugins:/home/ros2_ws/src/turtlebot3-UE/Plugins/RapyutaSimulationPlugins -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash -c "./nvidia-driver-check.sh '$DRIVER' && bash" +sudo nvidia-docker run --net host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name smacc_ue -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash -c "./nvidia-driver-check.sh '$DRIVER' && bash" diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_bash_development.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_bash_development.sh new file mode 100755 index 000000000..b33a9337d --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_bash_development.sh @@ -0,0 +1,8 @@ +#!/bin/sh +xhost + + +DIR="$(dirname "$(realpath "$0")")" +ROOT_DIR=`realpath $DIR/../../../..` +DRIVER=$(apt list --installed | grep nvidia-driver) + +sudo nvidia-docker run --net host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name smacc_ue -v $ROOT_DIR/ue_project_1:/home/ros2_ws/src/ue_project_1 -v $ROOT_DIR/SMACC2:/home/ros2_ws/src/SMACC2 -v $ROOT_DIR/UE-Plugins:/home/ros2_ws/src/ue_project_1/Plugins/RapyutaSimulationPlugins -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash -c "./nvidia-driver-check.sh '$DRIVER' && bash" diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_editor.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_editor.sh index bacdfc1d7..2e93b8c7e 100755 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_editor.sh +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_editor.sh @@ -5,4 +5,4 @@ DIR="$(dirname "$(realpath "$0")")" ROOT_DIR=`realpath $DIR/../../../..` DRIVER=$(apt list --installed | grep nvidia-driver) -sudo nvidia-docker run --net host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name smacc_ue -v $ROOT_DIR/SMACC2:/home/ros2_ws/src/SMACC2 -v $ROOT_DIR/UE-Plugins:/home/ros2_ws/src/turtlebot3-UE/Plugins/RapyutaSimulationPlugins -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash -c "./nvidia-driver-check.sh '$DRIVER' && ./run_editor_smacc.sh" +sudo nvidia-docker run --net host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name smacc_ue -v $ROOT_DIR/ue_project_2:/home/ros2_ws/src/ue_project_2 -v $ROOT_DIR/ue_project_1:/home/ros2_ws/src/ue_project_1 -v $ROOT_DIR/SMACC2:/home/ros2_ws/src/SMACC2 -v $ROOT_DIR/UE-Plugins:/home/ros2_ws/src/turtlebot3-UE/Plugins/RapyutaSimulationPlugins -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash -c "./nvidia-driver-check.sh '$DRIVER' && ./run_editor_smacc.sh" From bfc2e638fc53a94ff0e5ba91d339f31a13a865d4 Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Mon, 10 Jul 2023 18:22:55 +0000 Subject: [PATCH 42/70] finishing environment --- .../sm_dancebot_ue/docker/Dockerfile | 73 +++++++++---------- .../docker/save_current_docker_image.sh | 10 +++ 2 files changed, 46 insertions(+), 37 deletions(-) create mode 100755 smacc2_sm_reference_library/sm_dancebot_ue/docker/save_current_docker_image.sh diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile b/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile index 78078fef0..b260e84ef 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile @@ -63,7 +63,7 @@ RUN sudo sh -c 'echo "deb [arch=amd64,arm64,armhf signed-by=/etc/apt/keyrings/pa RUN sudo apt install -y apt-transport-https RUN sudo apt update -RUN sudo apt install -y code git-lfs # or code-insiders +RUN sudo apt install -y code git-lfs nano # or code-insiders # smacc2_devel RUN git clone --recurse-submodules https://github.com/robosoft-ai/${PROJECT_NAME}.git -b $PROJECT_BRANCH @@ -79,8 +79,8 @@ RUN git-lfs pull && git submodule foreach git-lfs pull RUN ls && echo ${UE5_DIR} RUN ./update_project_files.sh ENV UE5_DIR=/home/$USER/src/UE5.1 -RUN sudo apt-get install nano +# installing Dependencies RUN wget http://security.ubuntu.com/ubuntu/pool/universe/t/tinyxml2/libtinyxml2-6a_7.0.0+dfsg-1build1_amd64.deb RUN wget http://archive.ubuntu.com/ubuntu/pool/main/o/openssl/libssl1.1_1.1.0g-2ubuntu4_amd64.deb RUN sudo dpkg -i libtinyxml2-6a_7.0.0+dfsg-1build1_amd64.deb @@ -91,42 +91,41 @@ ENV UE5_DIR=/home/$USER/src/UE5.1/UnrealEngine COPY --chown=$USER:$USER smacc2_sm_reference_library/sm_dancebot_ue/docker/nvidia-driver-check.sh /home/$USER/src/${PROJECT_NAME}/nvidia-driver-check.sh RUN cat Makefile - -RUN make $PROJECT_NAME - - # COPY smacc2 /home/$USER/src/SMACC2/smacc2 -RUN mkdir -p /home/$USER/src/SMACC2/ -WORKDIR "/home/$USER/src/SMACC2" -COPY --chown=$USER:$USER smacc2_client_library /home/$USER/src/SMACC2/smacc2_client_library -COPY --chown=$USER:$USER smacc2_event_generator_library /home/$USER/src/SMACC2/smacc2_event_generator_library -COPY --chown=$USER:$USER smacc2_performance_tools /home/$USER/src/SMACC2/smacc2_performance_tools -COPY --chown=$USER:$USER smacc2_state_reactor_library /home/$USER/src/SMACC2/smacc2_state_reactor_library -COPY --chown=$USER:$USER smacc2 /home/$USER/src/SMACC2/smacc2 -COPY --chown=$USER:$USER smacc2_dev_tools /home/$USER/src/SMACC2/smacc2_dev_tools -COPY --chown=$USER:$USER smacc2_msgs /home/$USER/src/SMACC2/smacc2_msgs -#smacc2_sm_reference_library - -#RUN git clone -RUN sudo apt-get update -RUN rosdep update -RUN rosdep install --ignore-src --from-paths . -y -r -RUN sudo apt-get update && sudo apt-get install -y ros-humble-nav2-bringup ros-humble-rviz2 - -RUN sudo apt-get install -y openvpn -#COPY --chown=$USER:$USER smacc2_sm_reference_library/sm_dancebot_ue/docker/run_editor_smacc.sh /home/$USER/src/${PROJECT_NAME}/run_editor_smacc.sh - - -RUN touch /home/$USER/src/${PROJECT_NAME}/COLCON_IGNORE -RUN touch /home/$USER/src/UE5.1/COLCON_IGNORE -WORKDIR "/home/$USER/" -RUN bash -c "source /opt/ros/humble/setup.bash && colcon build --symlink-install" - +RUN ./update_project_files.sh +#RUN bash + +RUN make ${PROJECT_NAME}Editor + +# # COPY smacc2 /home/$USER/src/SMACC2/smacc2 +# RUN mkdir -p /home/$USER/src/SMACC2/ +# WORKDIR "/home/$USER/src/SMACC2" +# COPY --chown=$USER:$USER smacc2_client_library /home/$USER/src/SMACC2/smacc2_client_library +# COPY --chown=$USER:$USER smacc2_event_generator_library /home/$USER/src/SMACC2/smacc2_event_generator_library +# COPY --chown=$USER:$USER smacc2_performance_tools /home/$USER/src/SMACC2/smacc2_performance_tools +# COPY --chown=$USER:$USER smacc2_state_reactor_library /home/$USER/src/SMACC2/smacc2_state_reactor_library +# COPY --chown=$USER:$USER smacc2 /home/$USER/src/SMACC2/smacc2 +# COPY --chown=$USER:$USER smacc2_dev_tools /home/$USER/src/SMACC2/smacc2_dev_tools +# COPY --chown=$USER:$USER smacc2_msgs /home/$USER/src/SMACC2/smacc2_msgs +# #smacc2_sm_reference_library + +# #RUN git clone +# RUN sudo apt-get update +# RUN rosdep update +# RUN rosdep install --ignore-src --from-paths . -y -r +# RUN sudo apt-get update && sudo apt-get install -y ros-humble-nav2-bringup ros-humble-rviz2 + +# RUN sudo apt-get install -y openvpn +# #COPY --chown=$USER:$USER smacc2_sm_reference_library/sm_dancebot_ue/docker/run_editor_smacc.sh /home/$USER/src/${PROJECT_NAME}/run_editor_smacc.sh + + +# RUN touch /home/$USER/src/${PROJECT_NAME}/COLCON_IGNORE +# RUN touch /home/$USER/src/UE5.1/COLCON_IGNORE +# WORKDIR "/home/$USER/" +# RUN bash -c "source /opt/ros/humble/setup.bash && colcon build --symlink-install" + +# # BASIC VPN FRAMEWORK --- RUN mkdir /home/$USER/ovpnconfig -# BASIC VPN FRAMEWORK --- -CMD openvpn --daemon --config /home/$USER/ovpnconfig/ovpnconfig.ovpn --auth-user-pass /home/$USER/ovpnconfig/login.txt --askpass /home/$USER/ovpnconfig/password.conf WORKDIR "/home/$USER/src/${PROJECT_NAME}" -#ENV LD_LIBRARY_PATH=/home/ros2_ws/src/ue_project_1/Plugins/rclUE/ThirdParty/ros/lib/:/usr/local/cuda/lib64:/usr/lib/x86_64-linux-gnu/gazebo-11/plugins:/opt/ros/humble/opt/rviz_ogre_vendor/lib:/opt/ros/humble/lib/x86_64-linux-gnu: - -#ENTRYPOINT /home/$USER/src/ue_project_1/run_editor_smacc.sh +# #ENV LD_LIBRARY_PATH=/home/ros2_ws/src/ue_project_1/Plugins/rclUE/ThirdParty/ros/lib/:/usr/local/cuda/lib64:/usr/lib/x86_64-linux-gnu/gazebo-11/plugins:/opt/ros/humble/opt/rviz_ogre_vendor/lib:/opt/ros/humble/lib/x86_64-linux-gnu: \ No newline at end of file diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/save_current_docker_image.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/save_current_docker_image.sh new file mode 100755 index 000000000..95f35b8b6 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/save_current_docker_image.sh @@ -0,0 +1,10 @@ +#!/bin/bash + +OUTPUT_PATH=${1} + +if [ -z "$OUTPUT_PATH" ]; then + echo "Usage: $0 " + exit 1 +fi + +docker save -o "$OUTPUT_PATH/unreal_editor_smacc_${date +%Y%m%d_%H%M%S}.tar" ue_editor_rclue:humble \ No newline at end of file From d83b2d239eaac895dcfc5e17e7e3ec1106bdf9b4 Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Mon, 10 Jul 2023 18:26:27 +0000 Subject: [PATCH 43/70] fix save script --- .../sm_dancebot_ue/docker/save_current_docker_image.sh | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/save_current_docker_image.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/save_current_docker_image.sh index 95f35b8b6..61e443cd8 100755 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/save_current_docker_image.sh +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/save_current_docker_image.sh @@ -7,4 +7,5 @@ if [ -z "$OUTPUT_PATH" ]; then exit 1 fi -docker save -o "$OUTPUT_PATH/unreal_editor_smacc_${date +%Y%m%d_%H%M%S}.tar" ue_editor_rclue:humble \ No newline at end of file +DATE_SUFFIX=`date +%Y%m%d_%H%M%S` +docker save -o "$OUTPUT_PATH/unreal_editor_smacc_$DATE_SUFFIX.tar" ue_editor_rclue:humble \ No newline at end of file From 696e7d12b28aef7065c99ec66de8df7b74961589 Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Mon, 10 Jul 2023 19:09:52 +0000 Subject: [PATCH 44/70] fixing format --- smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile | 2 +- .../sm_dancebot_ue/docker/save_current_docker_image.sh | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile b/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile index b260e84ef..3302e7fa0 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile @@ -128,4 +128,4 @@ RUN mkdir /home/$USER/ovpnconfig WORKDIR "/home/$USER/src/${PROJECT_NAME}" -# #ENV LD_LIBRARY_PATH=/home/ros2_ws/src/ue_project_1/Plugins/rclUE/ThirdParty/ros/lib/:/usr/local/cuda/lib64:/usr/lib/x86_64-linux-gnu/gazebo-11/plugins:/opt/ros/humble/opt/rviz_ogre_vendor/lib:/opt/ros/humble/lib/x86_64-linux-gnu: \ No newline at end of file +# #ENV LD_LIBRARY_PATH=/home/ros2_ws/src/ue_project_1/Plugins/rclUE/ThirdParty/ros/lib/:/usr/local/cuda/lib64:/usr/lib/x86_64-linux-gnu/gazebo-11/plugins:/opt/ros/humble/opt/rviz_ogre_vendor/lib:/opt/ros/humble/lib/x86_64-linux-gnu: diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/save_current_docker_image.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/save_current_docker_image.sh index 61e443cd8..3cd242149 100755 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/save_current_docker_image.sh +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/save_current_docker_image.sh @@ -8,4 +8,4 @@ if [ -z "$OUTPUT_PATH" ]; then fi DATE_SUFFIX=`date +%Y%m%d_%H%M%S` -docker save -o "$OUTPUT_PATH/unreal_editor_smacc_$DATE_SUFFIX.tar" ue_editor_rclue:humble \ No newline at end of file +docker save -o "$OUTPUT_PATH/unreal_editor_smacc_$DATE_SUFFIX.tar" ue_editor_rclue:humble From c3049c0991a8e1fa53e972a89b4d36e002d5ee75 Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Mon, 10 Jul 2023 19:17:01 +0000 Subject: [PATCH 45/70] missing --- smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md b/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md index 3973b6c1e..51f64db26 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md @@ -117,7 +117,7 @@ If you want to modify the Docker image using a Dockerfile and rebuild it, follow 1. Create a link to the UE5.1 folder inside the `sm_dancebot_ue/docker` folder: ``` - mount --bind UE5.1/UnrealEngine + mount --bind UE5.1/UnrealEngine/Engine ``` 2. Build the image locally by running the following command: From cfb0a9d58f67bd10d253e26e83e6337cf9b18d16 Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Mon, 10 Jul 2023 19:37:04 +0000 Subject: [PATCH 46/70] missing --- smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md b/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md index 51f64db26..30eb2c178 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md @@ -117,8 +117,9 @@ If you want to modify the Docker image using a Dockerfile and rebuild it, follow 1. Create a link to the UE5.1 folder inside the `sm_dancebot_ue/docker` folder: ``` - mount --bind UE5.1/UnrealEngine/Engine + mount --bind UE5.1/UnrealEngine ``` + * Recall the must contain the following folders: `Engine`, `FeaturePacks` and `Template` 2. Build the image locally by running the following command: ``` From bf5223303d8a101e8b0937a459c4a995de8d9ebf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pablo=20I=C3=B1igo=20Blasco?= Date: Thu, 27 Jul 2023 22:52:35 +0200 Subject: [PATCH 47/70] rough terrain navigation and behaviors and ice winter demo --- .../cp_topic_subscriber.hpp | 8 +- .../nav2z_client/nav2z_client/CMakeLists.txt | 2 + .../cp_waypoints_navigator.hpp | 49 +-- .../cp_waypoints_navigator_base.hpp | 101 +++++++ .../nav2z_client/nav2z_client/package.xml | 1 + .../cp_waypoints_navigator.cpp | 87 ++++-- smacc2_dev_tools/.vscode/launch.json | 25 ++ .../states/st_event_count_down.hpp | 58 ---- .../sm_dancebot_ue/CMakeLists.txt | 7 +- .../run_docker_container_bash_development.sh | 2 +- .../sm_dancebot_ue/clients/cl_led/cl_led.hpp | 44 --- .../cl_led/client_behaviors/cb_led_off.hpp | 50 ---- .../cl_led/client_behaviors/cb_led_on.hpp | 50 ---- .../clients/cl_lidar/cl_lidar.hpp | 38 --- .../cb_load_waypoints_file.hpp | 63 ++++ .../cb_navigate_next_waypoint_free.hpp | 60 ++++ .../cb_position_control_free_space.hpp} | 44 ++- .../clients/cl_service3/cl_service3.hpp | 35 --- .../client_behaviors/cb_service3.hpp | 57 ---- .../cl_string_publisher.hpp | 40 --- .../cl_temperature_sensor.hpp | 38 --- ...cb_custom_condition_temperature_sensor.hpp | 56 ---- .../cp_ue_pose.hpp} | 49 +-- .../sm_dancebot_ue/orthogonals/or_led.hpp | 35 --- .../orthogonals/or_navigation.hpp | 55 +--- .../orthogonals/or_obstacle_perception.hpp | 7 +- .../orthogonals/or_service3.hpp | 37 --- .../orthogonals/or_string_publisher.hpp | 32 -- .../orthogonals/or_temperature_sensor.hpp | 43 --- .../sm_dancebot_ue/orthogonals/or_timer.hpp | 36 --- .../orthogonals/or_updatable_publisher.hpp | 37 --- .../include/sm_dancebot_ue/sm_dancebot_ue.hpp | 71 +---- .../sti_fpattern_forward_1.hpp | 1 - .../sti_fpattern_forward_2.hpp | 1 - .../sti_fpattern_return_1.hpp | 1 - .../sti_fpattern_rotate_1.hpp | 1 - .../sti_fpattern_rotate_2.hpp | 1 - .../sti_radial_end_point.hpp | 1 - .../sti_radial_return.hpp | 1 - .../sti_radial_rotate.hpp | 1 - .../sti_spattern_forward_1.hpp | 1 - .../sti_spattern_forward_2.hpp | 1 - .../sti_spattern_forward_3.hpp | 1 - .../sti_spattern_forward_4.hpp | 1 - .../sti_spattern_rotate_1.hpp | 2 - .../sti_spattern_rotate_2.hpp | 1 - .../sti_spattern_rotate_3.hpp | 1 - .../sti_spattern_rotate_4.hpp | 1 - .../states/st_acquire_sensors.hpp | 12 +- ..._x.hpp => st_back_on_road_waypoints_x.hpp} | 17 +- .../states/st_event_count_down.hpp | 14 +- ...e_3.hpp => st_inital_road_waypoints_x.hpp} | 36 ++- ....hpp => st_navigate_field_waypoints_x.hpp} | 36 ++- .../states/st_navigate_forward_1.hpp | 70 ----- .../states/st_navigate_forward_2.hpp | 39 --- .../states/st_navigate_reverse_2.hpp | 49 --- .../states/st_navigate_reverse_4.hpp | 49 --- .../states/st_navigate_to_waypoint_1.hpp | 49 --- .../states/st_rotate_degrees_1.hpp | 49 --- .../states/st_rotate_degrees_2.hpp | 48 --- .../states/st_rotate_degrees_3.hpp | 49 --- .../states/st_rotate_degrees_4.hpp | 48 --- .../states/st_rotate_degrees_5.hpp | 48 --- .../states/st_rotate_degrees_6.hpp | 48 --- .../superstates/ss_f_pattern_1.hpp | 3 +- .../superstates/ss_radial_pattern_1.hpp | 3 +- .../superstates/ss_radial_pattern_2.hpp | 3 +- .../superstates/ss_radial_pattern_3.hpp | 3 +- .../superstates/ss_s_pattern_1.hpp | 3 +- .../sm_dancebot_ue/package.xml | 1 + .../waypoints_plan_back_on_road.yaml | 282 ++++++++++++++++++ .../waypoints_plan_initial_road.yaml | 251 ++++++++++++++++ .../waypoints_plan_navigate_field.yaml | 111 +++++++ .../params/sm_dance_bot_config.yaml | 9 - .../params/sm_dancebot_ue_config.yaml | 12 + .../sm_dancebot_ue/clients/cl_led/cl_led.cpp | 46 --- .../cb_position_control_free_space.cpp | 205 +++++++++++++ .../clients/components/cp_ue_pose.cpp | 59 ++++ 78 files changed, 1405 insertions(+), 1581 deletions(-) create mode 100644 smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator_base.hpp delete mode 100644 smacc2_sm_reference_library/sm_dance_bot/include/sm_dance_bot/states/st_event_count_down.hpp delete mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_led/cl_led.hpp delete mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_led/client_behaviors/cb_led_off.hpp delete mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_led/client_behaviors/cb_led_on.hpp delete mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_lidar/cl_lidar.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_load_waypoints_file.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_navigate_next_waypoint_free.hpp rename smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/{cl_lidar/client_behaviors/cb_lidar_sensor.hpp => cl_nav2z/client_behaviors/cb_position_control_free_space.hpp} (56%) delete mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_service3/cl_service3.hpp delete mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_service3/client_behaviors/cb_service3.hpp delete mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_string_publisher/cl_string_publisher.hpp delete mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_temperature_sensor/cl_temperature_sensor.hpp delete mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_temperature_sensor/client_behaviors/cb_custom_condition_temperature_sensor.hpp rename smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/{cl_string_publisher/client_behaviors/cb_string_publisher.hpp => components/cp_ue_pose.hpp} (53%) delete mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_led.hpp delete mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_service3.hpp delete mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_string_publisher.hpp delete mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_temperature_sensor.hpp delete mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_timer.hpp delete mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_updatable_publisher.hpp rename smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/{st_navigate_to_waypoints_x.hpp => st_back_on_road_waypoints_x.hpp} (69%) rename smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/{st_navigate_reverse_3.hpp => st_inital_road_waypoints_x.hpp} (54%) rename smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/{st_navigate_reverse_1.hpp => st_navigate_field_waypoints_x.hpp} (54%) delete mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_forward_1.hpp delete mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_forward_2.hpp delete mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_reverse_2.hpp delete mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_reverse_4.hpp delete mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_to_waypoint_1.hpp delete mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_rotate_degrees_1.hpp delete mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_rotate_degrees_2.hpp delete mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_rotate_degrees_3.hpp delete mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_rotate_degrees_4.hpp delete mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_rotate_degrees_5.hpp delete mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_rotate_degrees_6.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan_back_on_road.yaml create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan_initial_road.yaml create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan_navigate_field.yaml delete mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/params/sm_dance_bot_config.yaml create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/params/sm_dancebot_ue_config.yaml delete mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/clients/cl_led/cl_led.cpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/clients/client_behaviors/nav2z_client/cb_position_control_free_space.cpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/clients/components/cp_ue_pose.cpp diff --git a/smacc2/include/smacc2/client_base_components/cp_topic_subscriber.hpp b/smacc2/include/smacc2/client_base_components/cp_topic_subscriber.hpp index 82602b2df..2e9550b18 100644 --- a/smacc2/include/smacc2/client_base_components/cp_topic_subscriber.hpp +++ b/smacc2/include/smacc2/client_base_components/cp_topic_subscriber.hpp @@ -44,12 +44,13 @@ class CpTopicSubscriber : public smacc2::ISmaccComponent virtual ~CpTopicSubscriber() {} - smacc2::SmaccSignal onFirstMessageReceived_; - smacc2::SmaccSignal onMessageReceived_; - std::function postMessageEvent; std::function postInitialMessageEvent; + smacc2::SmaccSignal onFirstMessageReceived_; + smacc2::SmaccSignal onMessageReceived_; + + // signal subscription method. This signal will be triggered when the first message is received template boost::signals2::connection onMessageReceived( void (T::*callback)(const MessageType &), T * object) @@ -57,6 +58,7 @@ class CpTopicSubscriber : public smacc2::ISmaccComponent return this->getStateMachine()->createSignalConnection(onMessageReceived_, callback, object); } + // signal subscription method. This signal will be triggered when the first message is received template boost::signals2::connection onFirstMessageReceived( void (T::*callback)(const MessageType &), T * object) diff --git a/smacc2_client_library/nav2z_client/nav2z_client/CMakeLists.txt b/smacc2_client_library/nav2z_client/nav2z_client/CMakeLists.txt index 3835806d4..172c36af0 100644 --- a/smacc2_client_library/nav2z_client/nav2z_client/CMakeLists.txt +++ b/smacc2_client_library/nav2z_client/nav2z_client/CMakeLists.txt @@ -24,6 +24,7 @@ find_package(tf2_geometry_msgs REQUIRED) find_package(pluginlib REQUIRED) find_package(bond REQUIRED) find_package(slam_toolbox REQUIRED) +find_package(ament_index_cpp REQUIRED) set(dependencies smacc2 @@ -37,6 +38,7 @@ set(dependencies tf2_geometry_msgs bond slam_toolbox + ament_index_cpp ) include_directories(include) diff --git a/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.hpp b/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.hpp index 192d7b3bd..37ccf51e5 100644 --- a/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.hpp +++ b/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.hpp @@ -24,25 +24,12 @@ #include #include +#include namespace cl_nav2z { class ClNav2Z; -struct Pose2D -{ - Pose2D(double x, double y, double yaw) - { - this->x_ = x; - this->y_ = y; - this->yaw_ = yaw; - } - - double x_; - double y_; - double yaw_; -}; - struct NavigateNextWaypointOptions { std::optional controllerName_; @@ -52,11 +39,9 @@ struct NavigateNextWaypointOptions // This component contains a list of waypoints. These waypoints can // be iterated in the different states using CbNextWaiPoint // waypoint index is only incremented if the current waypoint is successfully reached -class CpWaypointNavigator : public smacc2::ISmaccComponent +class CpWaypointNavigator : public CpWaypointNavigatorBase { public: - WaypointEventDispatcher waypointsEventDispatcher; - ClNav2Z * client_; CpWaypointNavigator(); @@ -69,14 +54,6 @@ class CpWaypointNavigator : public smacc2::ISmaccComponent waypointsEventDispatcher.initialize(client_); } - void loadWayPointsFromFile(std::string filepath); - - void loadWayPointsFromFile2(std::string filepath); - - void setWaypoints(const std::vector & waypoints); - - void setWaypoints(const std::vector & waypoints); - std::optional > > > sendNextGoal( @@ -86,39 +63,17 @@ class CpWaypointNavigator : public smacc2::ISmaccComponent void stopWaitingResult(); - const std::vector & getWaypoints() const; - const std::vector & getWaypointNames() const; - std::optional getNamedPose(std::string name) const; - - long getCurrentWaypointIndex() const; - std::optional getCurrentWaypointName() const; - - long currentWaypoint_; - - void rewind(int count); - - void forward(int count); - void seekName(std::string name); - smacc2::SmaccSignal onNavigationRequestSucceded; smacc2::SmaccSignal onNavigationRequestAborted; smacc2::SmaccSignal onNavigationRequestCancelled; private: - void insertWaypoint(int index, geometry_msgs::msg::Pose & newpose); - - void removeWaypoint(int index); - void onNavigationResult(const ClNav2Z::WrappedResult & r); void onGoalReached(const ClNav2Z::WrappedResult & res); void onGoalCancelled(const ClNav2Z::WrappedResult & /*res*/); void onGoalAborted(const ClNav2Z::WrappedResult & /*res*/); - std::vector waypoints_; - - std::vector waypointsNames_; - boost::signals2::connection succeddedNav2ZClientConnection_; boost::signals2::connection abortedNav2ZClientConnection_; boost::signals2::connection cancelledNav2ZClientConnection_; diff --git a/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator_base.hpp b/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator_base.hpp new file mode 100644 index 000000000..3726d147e --- /dev/null +++ b/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator_base.hpp @@ -0,0 +1,101 @@ +// 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 + +namespace cl_nav2z +{ + +struct Pose2D +{ + Pose2D(double x, double y, double yaw) + { + this->x_ = x; + this->y_ = y; + this->yaw_ = yaw; + } + + double x_; + double y_; + double yaw_; +}; + +// This component contains a list of waypoints. These waypoints can +// be iterated in the different states using CbNextWaiPoint +// waypoint index is only incremented if the current waypoint is successfully reached +class CpWaypointNavigatorBase : public smacc2::ISmaccComponent +{ +public: + WaypointEventDispatcher waypointsEventDispatcher; + + CpWaypointNavigatorBase(); + + virtual ~CpWaypointNavigatorBase(); + + void onInitialize() override; + + // template + // void onOrthogonalAllocation() + // { + // waypointsEventDispatcher.initialize(client_); + // } + + void loadWayPointsFromFile(std::string filepath); + + void loadWayPointsFromFile2(std::string filepath); + + void setWaypoints(const std::vector & waypoints); + + void setWaypoints(const std::vector & waypoints); + + const std::vector & getWaypoints() const; + const std::vector & getWaypointNames() const; + std::optional getNamedPose(std::string name) const; + geometry_msgs::msg::Pose getPose(int index) const; + geometry_msgs::msg::Pose getCurrentPose() const; + + + long getCurrentWaypointIndex() const; + std::optional getCurrentWaypointName() const; + + long currentWaypoint_; + + void rewind(int count); + + void forward(int count); + void seekName(std::string name); + + void loadWaypointsFromYamlParameter(std::string parameter_name, std::string yaml_file_package_name); + +protected: + void insertWaypoint(int index, geometry_msgs::msg::Pose & newpose); + + void removeWaypoint(int index); + + std::vector waypoints_; + + std::vector waypointsNames_; +}; +} // namespace cl_nav2z diff --git a/smacc2_client_library/nav2z_client/nav2z_client/package.xml b/smacc2_client_library/nav2z_client/nav2z_client/package.xml index 65dcd1643..bdcad8bd1 100644 --- a/smacc2_client_library/nav2z_client/nav2z_client/package.xml +++ b/smacc2_client_library/nav2z_client/nav2z_client/package.xml @@ -24,6 +24,7 @@ tf2_geometry_msgs bond slam_toolbox + ament_index_cpp ament_cmake diff --git a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.cpp b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.cpp index 4d0a62f09..9d26ff96e 100644 --- a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.cpp +++ b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.cpp @@ -21,6 +21,7 @@ #include #include +#include #include #include #include @@ -34,7 +35,13 @@ namespace cl_nav2z { using namespace std::chrono_literals; -CpWaypointNavigator::CpWaypointNavigator() : currentWaypoint_(0), waypoints_(0) {} +CpWaypointNavigatorBase::CpWaypointNavigatorBase() : currentWaypoint_(0), waypoints_(0) {} + +CpWaypointNavigatorBase::~CpWaypointNavigatorBase() {} + +CpWaypointNavigator::CpWaypointNavigator() {} + +void CpWaypointNavigatorBase::onInitialize() {} void CpWaypointNavigator::onInitialize() { client_ = dynamic_cast(owner_); } @@ -64,20 +71,20 @@ void CpWaypointNavigator::onGoalReached(const ClNav2Z::WrappedResult & /*res*/) onNavigationRequestSucceded(); } -void CpWaypointNavigator::rewind(int /*count*/) +void CpWaypointNavigatorBase::rewind(int /*count*/) { currentWaypoint_--; if (currentWaypoint_ < 0) currentWaypoint_ = 0; } -void CpWaypointNavigator::forward(int /*count*/) +void CpWaypointNavigatorBase::forward(int /*count*/) { currentWaypoint_++; if (currentWaypoint_ >= (long)waypoints_.size() - 1) currentWaypoint_ = (long)waypoints_.size() - 1; } -void CpWaypointNavigator::seekName(std::string name) +void CpWaypointNavigatorBase::seekName(std::string name) { bool found = false; @@ -140,6 +147,26 @@ void CpWaypointNavigator::seekName(std::string name) name.c_str(), previousWaypoint, currentWaypoint_); } +void CpWaypointNavigatorBase::loadWaypointsFromYamlParameter( + std::string parameter_name, std::string yaml_file_package_name) +{ + // if it is the first time and the waypoints navigator is not configured + std::string planfilepath; + getNode()->declare_parameter(parameter_name, planfilepath); + if (getNode()->get_parameter(parameter_name, planfilepath)) + { + std::string package_share_directory = + ament_index_cpp::get_package_share_directory(yaml_file_package_name); + boost::replace_all(planfilepath, "$(pkg_share)", package_share_directory); + this->loadWayPointsFromFile(planfilepath); + RCLCPP_INFO(getLogger(), "waypoints plan: %s", planfilepath.c_str()); + } + else + { + RCLCPP_ERROR(getLogger(), "waypoints plan file not found: NONE"); + } +} + void CpWaypointNavigator::stopWaitingResult() { if (succeddedNav2ZClientConnection_.connected()) @@ -278,7 +305,7 @@ void CpWaypointNavigator::onNavigationResult(const ClNav2Z::WrappedResult & r) } } -void CpWaypointNavigator::insertWaypoint(int index, geometry_msgs::msg::Pose & newpose) +void CpWaypointNavigatorBase::insertWaypoint(int index, geometry_msgs::msg::Pose & newpose) { if (index >= 0 && index <= (int)waypoints_.size()) { @@ -286,12 +313,12 @@ void CpWaypointNavigator::insertWaypoint(int index, geometry_msgs::msg::Pose & n } } -void CpWaypointNavigator::setWaypoints(const std::vector & waypoints) +void CpWaypointNavigatorBase::setWaypoints(const std::vector & waypoints) { this->waypoints_ = waypoints; } -void CpWaypointNavigator::setWaypoints(const std::vector & waypoints) +void CpWaypointNavigatorBase::setWaypoints(const std::vector & waypoints) { waypoints_.clear(); waypointsNames_.clear(); @@ -311,7 +338,7 @@ void CpWaypointNavigator::setWaypoints(const std::vector & waypoints) } } -void CpWaypointNavigator::removeWaypoint(int index) +void CpWaypointNavigatorBase::removeWaypoint(int index) { if (index >= 0 && index < (int)waypoints_.size()) { @@ -319,12 +346,36 @@ void CpWaypointNavigator::removeWaypoint(int index) } } -const std::vector & CpWaypointNavigator::getWaypoints() const +const std::vector & CpWaypointNavigatorBase::getWaypoints() const { return waypoints_; } -std::optional CpWaypointNavigator::getNamedPose(std::string name) const +geometry_msgs::msg::Pose CpWaypointNavigatorBase::getPose(int index) const +{ + if (index >= 0 && index < (int)waypoints_.size()) + { + return waypoints_[index]; + } + else + { + throw std::out_of_range("Waypoint index out of range"); + } +} +geometry_msgs::msg::Pose CpWaypointNavigatorBase::getCurrentPose() const +{ + if (currentWaypoint_ >= 0 && currentWaypoint_ < (int)waypoints_.size()) + { + return waypoints_[currentWaypoint_]; + } + else + { + throw std::out_of_range("Waypoint index out of range"); + } +} + +std::optional CpWaypointNavigatorBase::getNamedPose( + std::string name) const { if (this->waypointsNames_.size() > 0) { @@ -340,12 +391,12 @@ std::optional CpWaypointNavigator::getNamedPose(std::s return std::nullopt; } -const std::vector & CpWaypointNavigator::getWaypointNames() const +const std::vector & CpWaypointNavigatorBase::getWaypointNames() const { return waypointsNames_; } -std::optional CpWaypointNavigator::getCurrentWaypointName() const +std::optional CpWaypointNavigatorBase::getCurrentWaypointName() const { if (currentWaypoint_ >= 0 && currentWaypoint_ < (int)waypointsNames_.size()) { @@ -354,12 +405,12 @@ std::optional CpWaypointNavigator::getCurrentWaypointName() const return std::nullopt; } -long CpWaypointNavigator::getCurrentWaypointIndex() const { return currentWaypoint_; } +long CpWaypointNavigatorBase::getCurrentWaypointIndex() const { return currentWaypoint_; } #define HAVE_NEW_YAMLCPP -void CpWaypointNavigator::loadWayPointsFromFile(std::string filepath) +void CpWaypointNavigatorBase::loadWayPointsFromFile(std::string filepath) { - RCLCPP_INFO_STREAM(getLogger(), "[CpWaypointNavigator] Loading file:" << filepath); + RCLCPP_INFO_STREAM(getLogger(), "[CpWaypointNavigatorBase] Loading file:" << filepath); this->waypoints_.clear(); std::ifstream ifs(filepath.c_str(), std::ifstream::in); if (ifs.good() == false) @@ -413,7 +464,7 @@ void CpWaypointNavigator::loadWayPointsFromFile(std::string filepath) } catch (...) { - RCLCPP_ERROR(getLogger(), "parsing waypoint file, syntax error in point %d", i); + RCLCPP_ERROR(getLogger(), "parsing waypoint file, syntax error in point %ld", i); } } RCLCPP_INFO_STREAM(getLogger(), "Parsed " << this->waypoints_.size() << " waypoints."); @@ -430,7 +481,7 @@ void CpWaypointNavigator::loadWayPointsFromFile(std::string filepath) } } -void CpWaypointNavigator::loadWayPointsFromFile2(std::string filepath) +void CpWaypointNavigatorBase::loadWayPointsFromFile2(std::string filepath) { RCLCPP_INFO_STREAM(getLogger(), "[CpWaypointNavigator] Loading file:" << filepath); this->waypoints_.clear(); @@ -476,7 +527,7 @@ void CpWaypointNavigator::loadWayPointsFromFile2(std::string filepath) } catch (...) { - RCLCPP_ERROR(getLogger(), "parsing waypoint file, syntax error in point %d", i); + RCLCPP_ERROR(getLogger(), "parsing waypoint file, syntax error in point %ld", i); } } RCLCPP_INFO_STREAM(getLogger(), "Parsed " << this->waypoints_.size() << " waypoints."); diff --git a/smacc2_dev_tools/.vscode/launch.json b/smacc2_dev_tools/.vscode/launch.json index fdea2811b..afc766017 100644 --- a/smacc2_dev_tools/.vscode/launch.json +++ b/smacc2_dev_tools/.vscode/launch.json @@ -373,6 +373,31 @@ } ] }, + { + "name": "(gdb) SmDanceBotUE", + "type": "cppdbg", + "request": "launch", + "program": "${workspaceFolder}/../install/sm_dancebot_ue/lib/sm_dancebot_ue/sm_dancebot_ue_node", + "args": [ + "--ros-args", + "-r", + "__node:=SmDanceBot", + "--params-file", + "${workspaceFolder}/../install/sm_dancebot_ue/share/sm_dancebot_ue/params/sm_dancebot_ue_config.yaml" + ], + "stopAtEntry": false, + "cwd": "${workspaceFolder}", + "environment": [], + "externalConsole": false, + "MIMode": "gdb", + "setupCommands": [ + { + "description": "Enable pretty-printing for gdb", + "text": "-enable-pretty-printing", + "ignoreFailures": true + } + ] + }, { "name": "(gdb) SmDanceBotWarehouse2", "type": "cppdbg", diff --git a/smacc2_sm_reference_library/sm_dance_bot/include/sm_dance_bot/states/st_event_count_down.hpp b/smacc2_sm_reference_library/sm_dance_bot/include/sm_dance_bot/states/st_event_count_down.hpp deleted file mode 100644 index f7b9adf5a..000000000 --- a/smacc2_sm_reference_library/sm_dance_bot/include/sm_dance_bot/states/st_event_count_down.hpp +++ /dev/null @@ -1,58 +0,0 @@ -// 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 - -namespace sm_dance_bot -{ -// STATE DECLARATION -struct StEventCountDown : smacc2::SmaccState -{ - using SmaccState::SmaccState; - - // TRANSITION TABLE - typedef mpl::list< - - Transition, StNavigateToWaypointsX>, - Transition - - >reactions; - - // STATE FUNCTIONS - static void staticConfigure() - { - // configure_orthogonal(); - // configure_orthogonal("Hello World!"); - // configure_orthogonal(); - // configure_orthogonal(Service3Command::SERVICE3_ON); - - // Create State Reactor - - //auto srCountdown = static_createStateReactor(5); - //srCountdown->addInputEvent>(); - //srCountdown->setOutputEvent>(); - - auto srCountdown = static_createStateReactor< - SrEventCountdown, EvCountdownEnd, mpl::list>>(5); - } -}; -} // namespace sm_dance_bot diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/CMakeLists.txt b/smacc2_sm_reference_library/sm_dancebot_ue/CMakeLists.txt index 9c4d7eccc..e99db0db0 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/CMakeLists.txt +++ b/smacc2_sm_reference_library/sm_dancebot_ue/CMakeLists.txt @@ -28,6 +28,7 @@ find_package(sr_conditional REQUIRED) find_package(ros_timer_client REQUIRED) find_package(sensor_msgs REQUIRED) find_package(visualization_msgs REQUIRED) +find_package(ue_msgs REQUIRED) set(dependencies smacc2 @@ -41,6 +42,7 @@ set(dependencies sr_conditional ros_timer_client visualization_msgs + ue_msgs ) rosidl_generate_interfaces(${PROJECT_NAME} @@ -54,8 +56,9 @@ include_directories(include add_executable(${PROJECT_NAME}_node src/${PROJECT_NAME}/sm_dancebot_ue.cpp - src/${PROJECT_NAME}/clients/cl_led/cl_led.cpp -) + src/${PROJECT_NAME}/clients/client_behaviors/nav2z_client/cb_position_control_free_space.cpp + src/${PROJECT_NAME}/clients/components/cp_ue_pose.cpp + ) add_executable(temperature_sensor_node_${PROJECT_NAME} servers/temperature_sensor_node/src/temperature_sensor_node.cpp) add_executable(led_action_server_node_${PROJECT_NAME} servers/led_action_server/src/led_action_server_node.cpp) diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_bash_development.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_bash_development.sh index b33a9337d..c446c50a5 100755 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_bash_development.sh +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_bash_development.sh @@ -5,4 +5,4 @@ DIR="$(dirname "$(realpath "$0")")" ROOT_DIR=`realpath $DIR/../../../..` DRIVER=$(apt list --installed | grep nvidia-driver) -sudo nvidia-docker run --net host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name smacc_ue -v $ROOT_DIR/ue_project_1:/home/ros2_ws/src/ue_project_1 -v $ROOT_DIR/SMACC2:/home/ros2_ws/src/SMACC2 -v $ROOT_DIR/UE-Plugins:/home/ros2_ws/src/ue_project_1/Plugins/RapyutaSimulationPlugins -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash -c "./nvidia-driver-check.sh '$DRIVER' && bash" +sudo nvidia-docker run --net host --ipc=host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name smacc_ue -v $ROOT_DIR/ue_project_1:/home/ros2_ws/src/ue_project_1 -v $ROOT_DIR/SMACC2:/home/ros2_ws/src/SMACC2 -v $ROOT_DIR/UE-Plugins:/home/ros2_ws/src/ue_project_1/Plugins/RapyutaSimulationPlugins -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash -c "./nvidia-driver-check.sh '$DRIVER' && bash" diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_led/cl_led.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_led/cl_led.hpp deleted file mode 100644 index 78d0632e9..000000000 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_led/cl_led.hpp +++ /dev/null @@ -1,44 +0,0 @@ -// 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 - -namespace sm_dancebot_ue -{ -namespace cl_led -{ -class ClLED -: public smacc2::client_bases::SmaccActionClientBase -{ -public: - - ClLED(std::string actionServerName); - virtual std::string getName() const override; - virtual ~ClLED(); -}; - -std::ostream & operator<<( - std::ostream & out, const sm_dancebot_ue::action::LEDControl::Goal & msg); - -} // namespace cl_led -} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_led/client_behaviors/cb_led_off.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_led/client_behaviors/cb_led_off.hpp deleted file mode 100644 index 8ea3b3dc8..000000000 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_led/client_behaviors/cb_led_off.hpp +++ /dev/null @@ -1,50 +0,0 @@ -// 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 - -namespace sm_dancebot_ue -{ -namespace cl_led -{ -class CbLEDOff : public smacc2::SmaccClientBehavior -{ -public: - cl_led::ClLED * ledActionClient_; - - void onEntry() override - { - this->requiresClient(ledActionClient_); - - cl_led::ClLED::Goal goal; - goal.command = cl_led::ClLED::Goal::CMD_OFF; - ledActionClient_->sendGoal(goal); - } - - void onExit() override - { - //RCLCPP_INFO(getLogger(),"Entering ToolClientBehavior"); - } -}; -} // namespace cl_led -} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_led/client_behaviors/cb_led_on.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_led/client_behaviors/cb_led_on.hpp deleted file mode 100644 index 6a3d57bb2..000000000 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_led/client_behaviors/cb_led_on.hpp +++ /dev/null @@ -1,50 +0,0 @@ -// 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 - -namespace sm_dancebot_ue -{ -namespace cl_led -{ -class CbLEDOn : public smacc2::SmaccClientBehavior -{ -public: - cl_led::ClLED * ledActionClient_; - - void onEntry() override - { - this->requiresClient(ledActionClient_); - - cl_led::ClLED::Goal goal; - goal.command = cl_led::ClLED::Goal::CMD_ON; - ledActionClient_->sendGoal(goal); - } - - void onExit() override - { - //RCLCPP_INFO(getLogger(),"Entering ToolClientBehavior"); - } -}; -} // namespace cl_led -} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_lidar/cl_lidar.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_lidar/cl_lidar.hpp deleted file mode 100644 index 77d1f4cb7..000000000 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_lidar/cl_lidar.hpp +++ /dev/null @@ -1,38 +0,0 @@ -// 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 - -namespace sm_dancebot_ue -{ -namespace cl_lidar -{ -class ClLidarSensor : public cl_multirole_sensor::ClMultiroleSensor -{ -public: - ClLidarSensor() {} -}; -} // namespace cl_lidar -} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_load_waypoints_file.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_load_waypoints_file.hpp new file mode 100644 index 000000000..412703ad3 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_load_waypoints_file.hpp @@ -0,0 +1,63 @@ +// 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 + +namespace sm_dancebot_ue +{ +struct CbLoadWaypointsFile : public smacc2::SmaccClientBehavior +{ +public: + CbLoadWaypointsFile(std::string filepath) : filepath_(filepath) {} + + CbLoadWaypointsFile(std::string parameter_name, std::string packagenamesapce) + : + parameterName_(parameter_name), + packagenamesapce_(packagenamesapce) + { + } + + void onEntry() override + { + requiresComponent(waypointsNavigator_); // this is a component from the nav2z_client library + + if (filepath_) + this->waypointsNavigator_->loadWayPointsFromFile(filepath_.value()); + else + this->waypointsNavigator_->loadWaypointsFromYamlParameter( + parameterName_.value(), packagenamesapce_.value()); + + // change this to skip some points of the yaml file, default = 0 + waypointsNavigator_->currentWaypoint_ = 0; + } + + void onExit() override {} + + std::optional filepath_; + + std::optional parameterName_; + std::optional packagenamesapce_; + + cl_nav2z::CpWaypointNavigatorBase * waypointsNavigator_; +}; +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_navigate_next_waypoint_free.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_navigate_next_waypoint_free.hpp new file mode 100644 index 000000000..7a39cedf1 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_navigate_next_waypoint_free.hpp @@ -0,0 +1,60 @@ +// 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 + +namespace sm_dancebot_ue +{ + +class CbNavigateNextWaypointFree : public sm_dancebot_ue::CbPositionControlFreeSpace +{ +public: + CbNavigateNextWaypointFree() { } + + virtual ~CbNavigateNextWaypointFree() {} + + void onEntry() override + { + requiresComponent(this->waypointsNavigator_); + this->target_pose_ = this->waypointsNavigator_->getCurrentPose(); + + this->onSuccess(&CbNavigateNextWaypointFree::onSucessCallback, this); + RCLCPP_INFO_STREAM(getLogger(), "[CbNavigateNextWaypoint] initial load file target pose: x: " << this->target_pose_.position.x << ", y: " << this->target_pose_.position.y); + CbPositionControlFreeSpace::onEntry(); + + } + + void onSucessCallback() + { + RCLCPP_INFO_STREAM(getLogger(), "[CbNavigateNextWaypoint] Success on planning to next waypoint"); + this->waypointsNavigator_->forward(1); + RCLCPP_INFO_STREAM(getLogger(), "[CbNavigateNextWaypoint] next position index: " << this->waypointsNavigator_->getCurrentWaypointIndex() << "/" << this->waypointsNavigator_->getWaypoints().size()); + } + + void onExit() override + { + } + +protected: + cl_nav2z::CpWaypointNavigatorBase * waypointsNavigator_; +}; + +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_lidar/client_behaviors/cb_lidar_sensor.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_position_control_free_space.hpp similarity index 56% rename from smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_lidar/client_behaviors/cb_lidar_sensor.hpp rename to smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_position_control_free_space.hpp index fb60c30aa..f463ae303 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_lidar/client_behaviors/cb_lidar_sensor.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_position_control_free_space.hpp @@ -18,28 +18,44 @@ * ******************************************************************************************************************/ -#pragma once +#pragma once +#include +#include #include -#include -#include +#include +#include namespace sm_dancebot_ue { -namespace cl_lidar -{ -struct CbLidarSensor : cl_multirole_sensor::CbDefaultMultiRoleSensorBehavior +struct CbPositionControlFreeSpace : public smacc2::SmaccAsyncClientBehavior { +private: + double targetYaw_; + bool goalReached_; + double k_betta_; + double max_angular_yaw_speed_; + + double threshold_distance_; + double prev_error_linear_ = 0.0; + double prev_error_angular_ = 0.0; + double integral_linear_ = 0.0; + double integral_angular_ = 0.0; + + rclcpp::Publisher::SharedPtr cmd_vel_pub_; + public: - CbLidarSensor() {} + double yaw_goal_tolerance_rads_; + + geometry_msgs::msg::Pose target_pose_; + + CbPositionControlFreeSpace(); + + void updateParameters(); + + void onEntry() override; - void onEntry() override - { - RCLCPP_INFO(getLogger(), "[CbLidarSensor] onEntry"); - cl_multirole_sensor::CbDefaultMultiRoleSensorBehavior::onEntry(); - } + void onExit() override; - virtual void onMessageCallback(const sensor_msgs::msg::LaserScan & /*msg*/) override {} }; -} // namespace cl_lidar } // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_service3/cl_service3.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_service3/cl_service3.hpp deleted file mode 100644 index b54098119..000000000 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_service3/cl_service3.hpp +++ /dev/null @@ -1,35 +0,0 @@ -// 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 -namespace sm_dancebot_ue -{ -namespace cl_service3 -{ -class ClService3 : public smacc2::client_bases::SmaccServiceClient -{ -public: - ClService3() {} -}; -} // namespace cl_service3 -} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_service3/client_behaviors/cb_service3.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_service3/client_behaviors/cb_service3.hpp deleted file mode 100644 index 268ee9e24..000000000 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_service3/client_behaviors/cb_service3.hpp +++ /dev/null @@ -1,57 +0,0 @@ -// 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 - * - ******************************************************************************************************************/ - -#include -#include - -namespace sm_dancebot_ue -{ -namespace cl_service3 -{ -enum class Service3Command -{ - SERVICE3_ON, - SERVICE3_OFF -}; - -class CbService3 : public smacc2::SmaccClientBehavior -{ -private: - ClService3 * serviceClient_; - Service3Command value_; - -public: - CbService3(Service3Command value) { value_ = value; } - - void onEntry() override - { - this->requiresClient(serviceClient_); - - auto req = std::make_shared(); - if (value_ == Service3Command::SERVICE3_ON) - req->data = true; - else - req->data = false; - - serviceClient_->call(req); - } -}; -} // namespace cl_service3 -} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_string_publisher/cl_string_publisher.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_string_publisher/cl_string_publisher.hpp deleted file mode 100644 index 7d757c7a0..000000000 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_string_publisher/cl_string_publisher.hpp +++ /dev/null @@ -1,40 +0,0 @@ -// 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 - -namespace sm_dancebot_ue -{ -namespace cl_string_publisher -{ -class ClStringPublisher : public smacc2::client_bases::SmaccPublisherClient -{ -public: - ClStringPublisher(std::string topicName) : topicName_(topicName) {} - - void onInitialize() override { this->configure(topicName_); } - - std::string topicName_; -}; -} // namespace cl_string_publisher -} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_temperature_sensor/cl_temperature_sensor.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_temperature_sensor/cl_temperature_sensor.hpp deleted file mode 100644 index 2dbfba15d..000000000 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_temperature_sensor/cl_temperature_sensor.hpp +++ /dev/null @@ -1,38 +0,0 @@ -// 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 - -namespace sm_dancebot_ue -{ -namespace cl_temperature_sensor -{ -class ClTemperatureSensor -: public cl_multirole_sensor::ClMultiroleSensor -{ -public: - ClTemperatureSensor() {} -}; -} // namespace cl_temperature_sensor -} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_temperature_sensor/client_behaviors/cb_custom_condition_temperature_sensor.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_temperature_sensor/client_behaviors/cb_custom_condition_temperature_sensor.hpp deleted file mode 100644 index 47c6d9939..000000000 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_temperature_sensor/client_behaviors/cb_custom_condition_temperature_sensor.hpp +++ /dev/null @@ -1,56 +0,0 @@ -// 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 - -namespace sm_dancebot_ue -{ -namespace cl_temperature_sensor -{ -struct EvCustomTemperatureAlert : sc::event -{ -}; - -//-------------------------------------------------------------------------------------- -class CbConditionTemperatureSensor -: public cl_multirole_sensor::CbDefaultMultiRoleSensorBehavior -{ -public: - CbConditionTemperatureSensor() {} - void onEntry() override - { - RCLCPP_INFO(getLogger(), "[CbConditionTemperatureSensor] onEntry"); - cl_multirole_sensor::CbDefaultMultiRoleSensorBehavior::onEntry(); - } - - void onMessageCallback(const sensor_msgs::msg::Temperature & msg) override - { - if (msg.temperature > 40) - { - auto ev = new EvCustomTemperatureAlert(); - this->postEvent(ev); - } - } -}; -} // namespace cl_temperature_sensor -} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_string_publisher/client_behaviors/cb_string_publisher.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/components/cp_ue_pose.hpp similarity index 53% rename from smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_string_publisher/client_behaviors/cb_string_publisher.hpp rename to smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/components/cp_ue_pose.hpp index 5627d5e76..4db3b0a25 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_string_publisher/client_behaviors/cb_string_publisher.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/components/cp_ue_pose.hpp @@ -13,43 +13,44 @@ // limitations under the License. /***************************************************************************************************************** - * + *-2020 * Authors: Pablo Inigo Blasco, Brett Aldrich * ******************************************************************************************************************/ - #pragma once -#include -#include -#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include namespace sm_dancebot_ue { -namespace cl_string_publisher -{ -class CbStringPublisher : public smacc2::SmaccClientBehavior + +class CpUEPose : public smacc2::components::CpTopicSubscriber { public: - ClStringPublisher * publisherClient_; - std::string msg_; + CpUEPose(std::string topicname); - CbStringPublisher(std::string msg) { msg_ = msg; } + void onInitialize() override; + + void onPoseMessageReceived(const ue_msgs::msg::EntityState& msg); - virtual void runtimeConfigure() override - { - RCLCPP_INFO_STREAM( - getLogger(), "Creating CbStringPublisher behavior with stored message: " << msg_); - } + geometry_msgs::msg::Pose toPoseMsg(); - virtual void onEntry() { this->requiresClient(publisherClient_); } +private: + ue_msgs::msg::EntityState entityStateMsg_; - void onExit() override - { - std_msgs::msg::String rosmsg; - rosmsg.data = msg_; - publisherClient_->publish(rosmsg); - } }; -} // namespace cl_string_publisher } // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_led.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_led.hpp deleted file mode 100644 index c85084d3e..000000000 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_led.hpp +++ /dev/null @@ -1,35 +0,0 @@ -// 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 - -namespace sm_dancebot_ue -{ -class OrLED : public smacc2::Orthogonal -{ -public: - void onInitialize() override - { - auto actionclient = this->createClient("led_action_server"); - } -}; -} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_navigation.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_navigation.hpp index f664c132a..80e4d5067 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_navigation.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_navigation.hpp @@ -20,20 +20,14 @@ #pragma once -#include #include -#include - -#include -#include -#include -#include -#include +#include +#include namespace sm_dancebot_ue { -using namespace cl_nav2z; +using namespace ::cl_nav2z; using namespace std::chrono_literals; class OrNavigation : public smacc2::Orthogonal @@ -41,48 +35,11 @@ class OrNavigation : public smacc2::Orthogonal public: void onInitialize() override { - auto nav2zClient = this->createClient(); - - // create pose component - nav2zClient->createComponent(); - - // create planner switcher - nav2zClient->createComponent(); - - // create goal checker switcher - nav2zClient->createComponent(); - - // create odom tracker - nav2zClient->createComponent(); + auto nav2zClient = this->createClient(); - // create odom tracker - nav2zClient->createComponent(); + nav2zClient->createComponent("/ue_ros/map_origin_entity_state"); - // create waypoints navigator component - auto waypointsNavigator = nav2zClient->createComponent(); - loadWaypointsFromYaml(waypointsNavigator); - - // change this to skip some points of the yaml file, default = 0 - waypointsNavigator->currentWaypoint_ = 0; - } - - void loadWaypointsFromYaml(CpWaypointNavigator * waypointsNavigator) - { - // if it is the first time and the waypoints navigator is not configured - std::string planfilepath; - getNode()->declare_parameter("waypoints_plan", planfilepath); - if (getNode()->get_parameter("waypoints_plan", planfilepath)) - { - std::string package_share_directory = - ament_index_cpp::get_package_share_directory("sm_dancebot_ue"); - boost::replace_all(planfilepath, "$(pkg_share)", package_share_directory); - waypointsNavigator->loadWayPointsFromFile(planfilepath); - RCLCPP_INFO(getLogger(), "waypoints plan: %s", planfilepath.c_str()); - } - else - { - RCLCPP_ERROR(getLogger(), "waypoints plan file not found: NONE"); - } + auto waypointsNavigator = nav2zClient->createComponent<::cl_nav2z::CpWaypointNavigatorBase>(); } }; } // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_obstacle_perception.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_obstacle_perception.hpp index 57acbc326..449ee42ce 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_obstacle_perception.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_obstacle_perception.hpp @@ -20,7 +20,6 @@ #pragma once -#include #include namespace sm_dancebot_ue @@ -31,10 +30,10 @@ class OrObstaclePerception : public smacc2::Orthogonal public: void onInitialize() override { - auto lidarClient = this->createClient(); + // auto lidarClient = this->createClient(); - lidarClient->topicName = "/scan"; - lidarClient->timeout_ = rclcpp::Duration(10s); + // lidarClient->topicName = "/scan"; + // lidarClient->timeout_ = rclcpp::Duration(10s); } }; } // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_service3.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_service3.hpp deleted file mode 100644 index c9ef9f1d9..000000000 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_service3.hpp +++ /dev/null @@ -1,37 +0,0 @@ -// 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 - -namespace sm_dancebot_ue -{ -class OrService3 : public smacc2::Orthogonal -{ -public: - void onInitialize() override - { - auto serviceClient = this->createClient(); - serviceClient->serviceName_ = "/service_node3"; - } -}; -} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_string_publisher.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_string_publisher.hpp deleted file mode 100644 index 433cac9fb..000000000 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_string_publisher.hpp +++ /dev/null @@ -1,32 +0,0 @@ -// 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 -namespace sm_dancebot_ue -{ -class OrStringPublisher : public smacc2::Orthogonal -{ -public: - void onInitialize() override { this->createClient("/string_publisher_out"); } -}; -} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_temperature_sensor.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_temperature_sensor.hpp deleted file mode 100644 index d8e35742b..000000000 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_temperature_sensor.hpp +++ /dev/null @@ -1,43 +0,0 @@ -// 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 - -using namespace std::chrono_literals; - -namespace sm_dancebot_ue -{ -class OrTemperatureSensor : public smacc2::Orthogonal -{ -public: - void onInitialize() override - { - auto clTemperatureSensor = this->createClient(); - - clTemperatureSensor->topicName = "/temperature"; - clTemperatureSensor->timeout_ = rclcpp::Duration(10s); - } -}; -} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_timer.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_timer.hpp deleted file mode 100644 index 7ae1ad8d7..000000000 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_timer.hpp +++ /dev/null @@ -1,36 +0,0 @@ -// 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 - -namespace sm_dancebot_ue -{ -class OrTimer : public smacc2::Orthogonal -{ -public: - void onInitialize() override - { - auto actionclient = this->createClient( - rclcpp::Duration(std::chrono::milliseconds(500))); - } -}; -} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_updatable_publisher.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_updatable_publisher.hpp deleted file mode 100644 index 960bdf725..000000000 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_updatable_publisher.hpp +++ /dev/null @@ -1,37 +0,0 @@ -// 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 - -namespace sm_dancebot_ue -{ -class OrUpdatablePublisher : public smacc2::Orthogonal -{ -public: - void onInitialize() override - { - auto publisherClient_ = this->createClient(); - publisherClient_->configure("/updatable_string_publisher_out"); - } -}; -} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/sm_dancebot_ue.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/sm_dancebot_ue.hpp index 0f655137b..86c5daf0d 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/sm_dancebot_ue.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/sm_dancebot_ue.hpp @@ -26,36 +26,22 @@ // CLIENT BEHAVIORS #include -#include - #include #include -using namespace cl_nav2z; - -#include -#include +// #include +#include +#include +#include -#include -#include - -#include -#include +using namespace cl_nav2z; #include #include #include - - #include -using namespace sm_dancebot_ue::cl_lidar; -using namespace sm_dancebot_ue::cl_service3; -using namespace sm_dancebot_ue::cl_string_publisher; -using namespace sm_dancebot_ue::cl_temperature_sensor; -using namespace sm_dancebot_ue::cl_led; - //STATE REACTORS #include #include @@ -64,14 +50,8 @@ using namespace sm_dancebot_ue::cl_led; using namespace smacc2::state_reactors; // ORTHOGONALS -#include #include #include -#include -#include -#include -#include -#include namespace sm_dancebot_ue { @@ -79,21 +59,9 @@ namespace sm_dancebot_ue class StAcquireSensors; class StEventCountDown; -class StNavigateForward1; -class StNavigateToWaypoint1; -class StNavigateToWaypointsX; - -class StRotateDegrees1; -class StRotateDegrees2; -class StRotateDegrees3; -class StRotateDegrees4; -class StRotateDegrees5; -class StRotateDegrees6; - -class StNavigateReverse1; -class StNavigateReverse2; -class StNavigateReverse3; -class StNavigateReverse4; +class StInitialRoadWaypointsX; +class StNavigateFieldWaypointsX; +class StBackOnRoadWaypointsX; //SUPERSTATE FORWARD DECLARATIONS //MODE STATES FORWARD DECLARATIONS @@ -158,12 +126,6 @@ struct SmDanceBot : public smacc2::SmaccStateMachineBasecreateOrthogonal(); this->createOrthogonal(); - this->createOrthogonal(); - this->createOrthogonal(); - this->createOrthogonal(); - this->createOrthogonal(); - this->createOrthogonal(); - this->createOrthogonal(); } }; @@ -185,17 +147,6 @@ struct SmDanceBot : public smacc2::SmaccStateMachineBase #include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_forward_1.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_forward_1.hpp index a3e076242..068dbaacf 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_forward_1.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_forward_1.hpp @@ -44,7 +44,6 @@ struct StiFPatternForward1 : public smacc2::SmaccState, { TSti::template configure_orthogonal(SS::ray_lenght_meters()); TSti::template configure_orthogonal(); - TSti::template configure_orthogonal(); } void runtimeConfigure() diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_forward_2.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_forward_2.hpp index 2693c7600..41c9be845 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_forward_2.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_forward_2.hpp @@ -43,7 +43,6 @@ struct StiFPatternForward2 : smacc2::SmaccState, SS> { TSti::template configure_orthogonal(SS::pitch_lenght_meters()); TSti::template configure_orthogonal(); - TSti::template configure_orthogonal(); } void runtimeConfigure() {} diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_return_1.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_return_1.hpp index bd54a79fa..b13032859 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_return_1.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_return_1.hpp @@ -42,7 +42,6 @@ struct StiFPatternReturn1 : smacc2::SmaccState, SS> { TSti::template configure_orthogonal(); TSti::template configure_orthogonal(); - TSti::template configure_orthogonal(); } void runtimeConfigure() {} diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_rotate_1.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_rotate_1.hpp index c9a6c1945..f0f08adbb 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_rotate_1.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_rotate_1.hpp @@ -52,7 +52,6 @@ struct StiFPatternRotate1 : smacc2::SmaccState, SS> //TSti::template configure_orthogonal(angle); TSti::template configure_orthogonal( angle); // absolute aligned to the y-axis - TSti::template configure_orthogonal(); } void runtimeConfigure() {} diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_rotate_2.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_rotate_2.hpp index 84a25912a..f44301bb8 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_rotate_2.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_rotate_2.hpp @@ -52,7 +52,6 @@ struct StiFPatternRotate2 : smacc2::SmaccState, SS> //TSti::template configure_orthogonal(angle); TSti::template configure_orthogonal( 0 + offset); // absolute horizontal - TSti::template configure_orthogonal(); TSti::template configure_orthogonal(); } diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/radial_motion_states/sti_radial_end_point.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/radial_motion_states/sti_radial_end_point.hpp index db38080c1..26a457a00 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/radial_motion_states/sti_radial_end_point.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/radial_motion_states/sti_radial_end_point.hpp @@ -41,7 +41,6 @@ struct StiRadialEndPoint : smacc2::SmaccState //RCLCPP_INFO(getLogger(),"ssr radial end point, distance in meters: %lf", SS::ray_length_meters()); configure_orthogonal(SS::ray_length_meters()); configure_orthogonal(); - configure_orthogonal(); } void runtimeConfigure() {} diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/radial_motion_states/sti_radial_return.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/radial_motion_states/sti_radial_return.hpp index 8050092f9..adbd6119f 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/radial_motion_states/sti_radial_return.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/radial_motion_states/sti_radial_return.hpp @@ -40,7 +40,6 @@ struct StiRadialReturn : smacc2::SmaccState { configure_orthogonal(); configure_orthogonal(); - configure_orthogonal(); } void onExit() diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/radial_motion_states/sti_radial_rotate.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/radial_motion_states/sti_radial_rotate.hpp index 522de035e..cb14b8951 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/radial_motion_states/sti_radial_rotate.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/radial_motion_states/sti_radial_rotate.hpp @@ -40,7 +40,6 @@ struct StiRadialRotate : smacc2::SmaccState { configure_orthogonal(); configure_orthogonal(); - configure_orthogonal(); } void runtimeConfigure() diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_forward_1.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_forward_1.hpp index d10607157..3ace5d7fb 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_forward_1.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_forward_1.hpp @@ -38,7 +38,6 @@ struct StiSPatternForward1 : public smacc2::SmaccState // STATE FUNCTIONS static void staticConfigure() { - configure_orthogonal(); configure_orthogonal(SS::pitch1_lenght_meters()); configure_orthogonal(); } diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_forward_2.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_forward_2.hpp index a60d30f30..056283f03 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_forward_2.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_forward_2.hpp @@ -40,7 +40,6 @@ struct StiSPatternForward2 : public smacc2::SmaccState { configure_orthogonal(SS::pitch2_lenght_meters()); configure_orthogonal(); - configure_orthogonal(); } void runtimeConfigure() {} diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_forward_3.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_forward_3.hpp index f677f2ac5..9546b65ac 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_forward_3.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_forward_3.hpp @@ -40,7 +40,6 @@ struct StiSPatternForward3 : public smacc2::SmaccState { configure_orthogonal(SS::pitch1_lenght_meters()); configure_orthogonal(); - configure_orthogonal(); } }; } // namespace s_pattern_states diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_forward_4.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_forward_4.hpp index 2e33e06ae..6da920092 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_forward_4.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_forward_4.hpp @@ -44,7 +44,6 @@ struct StiSPatternForward4 : public smacc2::SmaccState this->configure(SS::pitch2_lenght_meters()); this->configure(); - this->configure(); } }; } // namespace s_pattern_states diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_rotate_1.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_rotate_1.hpp index d672586b8..a54600e68 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_rotate_1.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_rotate_1.hpp @@ -53,8 +53,6 @@ struct StiSPatternRotate1 : smacc2::SmaccState // - offset because we are looking to the south and we have to turn counter-clockwise this->configure(180 + offset); } - - this->configure(); } }; } // namespace s_pattern_states diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_rotate_2.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_rotate_2.hpp index e251df95f..782587112 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_rotate_2.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_rotate_2.hpp @@ -47,7 +47,6 @@ struct StiSPatternRotate2 : smacc2::SmaccState configure_orthogonal(angle); configure_orthogonal(); - configure_orthogonal(); } void runtimeConfigure() diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_rotate_3.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_rotate_3.hpp index ef78446c0..1919ddc39 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_rotate_3.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_rotate_3.hpp @@ -66,7 +66,6 @@ struct StiSPatternRotate3 : smacc2::SmaccState } this->configure(); - this->configure(); } }; } // namespace s_pattern_states diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_rotate_4.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_rotate_4.hpp index dd5e29168..5d1a9bbb2 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_rotate_4.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_rotate_4.hpp @@ -54,7 +54,6 @@ struct StiSPatternRotate4 : smacc2::SmaccState this->configure(angle); this->configure(); - this->configure(); } }; } // namespace s_pattern_states diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_acquire_sensors.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_acquire_sensors.hpp index 98c2a6c54..246a09df1 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_acquire_sensors.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_acquire_sensors.hpp @@ -40,7 +40,7 @@ struct StAcquireSensors : smacc2::SmaccState, StEventCountDown, ON_SENSORS_AVAILABLE>, + Transition, StInitialRoadWaypointsX, ON_SENSORS_AVAILABLE>, Transition >reactions; @@ -48,21 +48,23 @@ struct StAcquireSensors : smacc2::SmaccState(); // configure_orthogonal("Hello World!"); // configure_orthogonal(); // configure_orthogonal(Service3Command::SERVICE3_ON); // configure_orthogonal(); - configure_orthogonal(); + // configure_orthogonal(); + configure_orthogonal("waypoints_plan_initial_road", "sm_dancebot_ue"); configure_orthogonal(5s); + + + // Create State Reactor auto srAllSensorsReady = static_createStateReactor< SrAllEventsGo, smacc2::state_reactors::EvAllGo, mpl::list< - EvTopicMessage, // EvTopicMessage, - EvCbSuccess, + // EvCbSuccess, EvCbSuccess >>(); } diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_to_waypoints_x.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_back_on_road_waypoints_x.hpp similarity index 69% rename from smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_to_waypoints_x.hpp rename to smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_back_on_road_waypoints_x.hpp index 5062fb6c7..81cad34c0 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_to_waypoints_x.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_back_on_road_waypoints_x.hpp @@ -24,8 +24,9 @@ namespace sm_dancebot_ue { + // STATE DECLARATION -struct StNavigateToWaypointsX : smacc2::SmaccState +struct StBackOnRoadWaypointsX : smacc2::SmaccState { using SmaccState::SmaccState; @@ -39,20 +40,18 @@ struct StNavigateToWaypointsX : smacc2::SmaccState, StNavigateToWaypointsX, TRANSITION_1>, - Transition, StNavigateToWaypointsX, TRANSITION_2>, - Transition, SS1::SsRadialPattern1, TRANSITION_3>, - Transition, SS2::SsRadialPattern2, TRANSITION_4> + Transition, StBackOnRoadWaypointsX, TRANSITION_1> + // Transition, StNavigateToWaypointsX, TRANSITION_2>, + // Transition, SS1::SsRadialPattern1, TRANSITION_3>, + // Transition, SS2::SsRadialPattern2, TRANSITION_4> >reactions; // STATE FUNCTIONS static void staticConfigure() { - configure_orthogonal(); - configure_orthogonal(); - configure_orthogonal(); - configure_orthogonal(); + // configure_orthogonal(); + configure_orthogonal(); } void onEntry() diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_event_count_down.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_event_count_down.hpp index 4c40cf7c4..328b03ff3 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_event_count_down.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_event_count_down.hpp @@ -32,7 +32,7 @@ struct StEventCountDown : smacc2::SmaccState, StNavigateToWaypointsX>, + // Transition, StNavigateToWaypointsX>, Transition >reactions; @@ -40,19 +40,7 @@ struct StEventCountDown : smacc2::SmaccState(); - // configure_orthogonal("Hello World!"); - // configure_orthogonal(); - // configure_orthogonal(Service3Command::SERVICE3_ON); - // Create State Reactor - - //auto srCountdown = static_createStateReactor(5); - //srCountdown->addInputEvent>(); - //srCountdown->setOutputEvent>(); - - auto srCountdown = static_createStateReactor< - SrEventCountdown, EvCountdownEnd, mpl::list>>(5); } }; } // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_reverse_3.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_inital_road_waypoints_x.hpp similarity index 54% rename from smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_reverse_3.hpp rename to smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_inital_road_waypoints_x.hpp index 51be066bd..9128cd17f 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_reverse_3.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_inital_road_waypoints_x.hpp @@ -24,26 +24,44 @@ namespace sm_dancebot_ue { + // STATE DECLARATION -struct StNavigateReverse3 : smacc2::SmaccState +struct StInitialRoadWaypointsX : smacc2::SmaccState { using SmaccState::SmaccState; + // CUSTOM TRANSITION TAGS + struct TRANSITION_1 : SUCCESS{}; + struct TRANSITION_2 : SUCCESS{}; + struct TRANSITION_3 : SUCCESS{}; + struct TRANSITION_4 : SUCCESS{}; + struct TRANSITION_5 : SUCCESS{}; + struct TRANSITION_6 : SUCCESS{}; + // TRANSITION TABLE typedef mpl::list< - - Transition, StNavigateToWaypoint1>, - Transition, StNavigateToWaypointsX> - + Transition, StInitialRoadWaypointsX, TRANSITION_1> + // Transition, StNavigateToWaypointsX, TRANSITION_2>, + // Transition, SS1::SsRadialPattern1, TRANSITION_3>, + // Transition, SS2::SsRadialPattern2, TRANSITION_4> + >reactions; // STATE FUNCTIONS static void staticConfigure() { - configure_orthogonal(1); - configure_orthogonal(); - configure_orthogonal(); - configure_orthogonal(); + // configure_orthogonal(); + configure_orthogonal(); + } + + void onEntry() + { + } + + void runtimeConfigure() {} + + void onExit(ABORT) + { } }; } // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_reverse_1.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_field_waypoints_x.hpp similarity index 54% rename from smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_reverse_1.hpp rename to smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_field_waypoints_x.hpp index 1033ed3bd..429aa7d5a 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_reverse_1.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_field_waypoints_x.hpp @@ -24,26 +24,44 @@ namespace sm_dancebot_ue { + // STATE DECLARATION -struct StNavigateReverse1 : smacc2::SmaccState +struct StNavigateFieldWaypointsX : smacc2::SmaccState { using SmaccState::SmaccState; + // CUSTOM TRANSITION TAGS + struct TRANSITION_1 : SUCCESS{}; + struct TRANSITION_2 : SUCCESS{}; + struct TRANSITION_3 : SUCCESS{}; + struct TRANSITION_4 : SUCCESS{}; + struct TRANSITION_5 : SUCCESS{}; + struct TRANSITION_6 : SUCCESS{}; + // TRANSITION TABLE typedef mpl::list< - - Transition, StRotateDegrees3>, - Transition, StNavigateReverse1> - + Transition, StNavigateFieldWaypointsX, TRANSITION_1> + // Transition, StNavigateToWaypointsX, TRANSITION_2>, + // Transition, SS1::SsRadialPattern1, TRANSITION_3>, + // Transition, SS2::SsRadialPattern2, TRANSITION_4> + >reactions; // STATE FUNCTIONS static void staticConfigure() { - configure_orthogonal(1); - configure_orthogonal(); - configure_orthogonal(); - configure_orthogonal(); + // configure_orthogonal(); + configure_orthogonal(); + } + + void onEntry() + { + } + + void runtimeConfigure() {} + + void onExit(ABORT) + { } }; } // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_forward_1.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_forward_1.hpp deleted file mode 100644 index f985bc858..000000000 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_forward_1.hpp +++ /dev/null @@ -1,70 +0,0 @@ -// 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 - -namespace sm_dancebot_ue -{ -// STATE DECLARATION -struct StNavigateForward1 : smacc2::SmaccState -{ - using SmaccState::SmaccState; - - // TRANSITION TABLE - typedef mpl::list< - - Transition, StRotateDegrees2>, - Transition, StNavigateForward1, ABORT> - //, Transition, StNavigateToWaypointsX, PREEMPT> - - >reactions; - - // STATE FUNCTIONS - static void staticConfigure() - { - configure_orthogonal(1); - configure_orthogonal(); - configure_orthogonal(); - configure_orthogonal(); - - } - - void runtimeConfigure() - { - ClNav2Z * move_base_action_client; - this->requiresClient(move_base_action_client); - - // we careful with the lifetime of the callbac, us a scoped connection if is not forever - move_base_action_client->onSucceeded(&StNavigateForward1::onActionClientSucceeded, this); - } - - void onActionClientSucceeded(cl_nav2z::ClNav2Z::WrappedResult & msg) - { - RCLCPP_INFO_STREAM( - getLogger(), - " [Callback SmaccSignal] Success Detected from StAcquireSensors (connected to client signal), " - "result data: " - << msg.result); - } -}; -} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_forward_2.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_forward_2.hpp deleted file mode 100644 index 4f0e5f80e..000000000 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_forward_2.hpp +++ /dev/null @@ -1,39 +0,0 @@ -// 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. - -#include -namespace sm_dancebot_ue -{ -// STATE DECLARATION -struct StNavigateForward2 : smacc2::SmaccState -{ - using SmaccState::SmaccState; - - // TRANSITION TABLE - typedef mpl::list< - - Transition, StRotateDegrees5>, - Transition, StNavigateToWaypointsX> - - >reactions; - - // STATE FUNCTIONS - static void staticConfigure() - { - configure_orthogonal(1); - configure_orthogonal(); - configure_orthogonal(); - } -}; -} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_reverse_2.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_reverse_2.hpp deleted file mode 100644 index 71ca57dbb..000000000 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_reverse_2.hpp +++ /dev/null @@ -1,49 +0,0 @@ -// 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 - -namespace sm_dancebot_ue -{ -// STATE DECLARATION -struct StNavigateReverse2 : smacc2::SmaccState -{ - using SmaccState::SmaccState; - - // TRANSITION TABLE - typedef mpl::list< - - Transition, StNavigateToWaypointsX>, - Transition, StNavigateToWaypointsX> - - >reactions; - - // STATE FUNCTIONS - static void staticConfigure() - { - configure_orthogonal(1); - configure_orthogonal(); - configure_orthogonal(); - configure_orthogonal(); - } -}; -} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_reverse_4.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_reverse_4.hpp deleted file mode 100644 index 302f14f61..000000000 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_reverse_4.hpp +++ /dev/null @@ -1,49 +0,0 @@ -// 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 - -namespace sm_dancebot_ue -{ -// STATE DECLARATION -struct StNavigateReverse4 : smacc2::SmaccState -{ - using SmaccState::SmaccState; - - // TRANSITION TABLE - typedef mpl::list< - - Transition, StRotateDegrees5>, - Transition, StNavigateToWaypointsX> - - >reactions; - - // STATE FUNCTIONS - static void staticConfigure() - { - configure_orthogonal(0.5); - configure_orthogonal(); - configure_orthogonal(); - configure_orthogonal(); - } -}; -} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_to_waypoint_1.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_to_waypoint_1.hpp deleted file mode 100644 index 1e24548e9..000000000 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_to_waypoint_1.hpp +++ /dev/null @@ -1,49 +0,0 @@ -// 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 - -namespace sm_dancebot_ue -{ -// STATE DECLARATION -struct StNavigateToWaypoint1 : smacc2::SmaccState -{ - using SmaccState::SmaccState; - - // TRANSITION TABLE - typedef mpl::list< - - Transition, StNavigateToWaypointsX>, - Transition, StNavigateToWaypoint1> - - >reactions; - - // STATE FUNCTIONS - static void staticConfigure() - { - configure_orthogonal(2.0, 0, 0); - configure_orthogonal(); - configure_orthogonal("All Done!"); - configure_orthogonal(); - } -}; -} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_rotate_degrees_1.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_rotate_degrees_1.hpp deleted file mode 100644 index 69dffce91..000000000 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_rotate_degrees_1.hpp +++ /dev/null @@ -1,49 +0,0 @@ -// 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 - -namespace sm_dancebot_ue -{ -// STATE DECLARATION -struct StRotateDegrees1 : smacc2::SmaccState -{ - using SmaccState::SmaccState; - - // TRANSITION TABLE - typedef mpl::list< - - Transition, StNavigateForward1>, - Transition, StNavigateToWaypointsX> - - >reactions; - - // STATE FUNCTIONS - static void staticConfigure() - { - configure_orthogonal(/*30*/ 90); - configure_orthogonal(); - configure_orthogonal(); - configure_orthogonal(); - } -}; -} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_rotate_degrees_2.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_rotate_degrees_2.hpp deleted file mode 100644 index b8631ae63..000000000 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_rotate_degrees_2.hpp +++ /dev/null @@ -1,48 +0,0 @@ -// 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 -namespace sm_dancebot_ue -{ -// STATE DECLARATION -struct StRotateDegrees2 : smacc2::SmaccState -{ - using SmaccState::SmaccState; - - // TRANSITION TABLE - typedef mpl::list< - - Transition, StNavigateToWaypointsX>, - Transition, StNavigateToWaypointsX> - - >reactions; - - // STATE FUNCTIONS - static void staticConfigure() - { - configure_orthogonal(/*30*/ -90); - configure_orthogonal(); - configure_orthogonal(); - configure_orthogonal(); - } -}; -} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_rotate_degrees_3.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_rotate_degrees_3.hpp deleted file mode 100644 index ecc48df26..000000000 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_rotate_degrees_3.hpp +++ /dev/null @@ -1,49 +0,0 @@ -// 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 - -namespace sm_dancebot_ue -{ -// STATE DECLARATION -struct StRotateDegrees3 : smacc2::SmaccState -{ - using SmaccState::SmaccState; - - // TRANSITION TABLE - typedef mpl::list< - - Transition, StNavigateToWaypointsX>, - Transition, StNavigateToWaypointsX> - - >reactions; - - // STATE FUNCTIONS - static void staticConfigure() - { - configure_orthogonal(/*30*/ 180); - configure_orthogonal(); - configure_orthogonal(); - configure_orthogonal(); - } -}; -} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_rotate_degrees_4.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_rotate_degrees_4.hpp deleted file mode 100644 index 1060dcd35..000000000 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_rotate_degrees_4.hpp +++ /dev/null @@ -1,48 +0,0 @@ -// 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 -namespace sm_dancebot_ue -{ -// STATE DECLARATION -struct StRotateDegrees4 : smacc2::SmaccState -{ - using SmaccState::SmaccState; - - // TRANSITION TABLE - typedef mpl::list< - - Transition, StNavigateReverse2>, - Transition, StNavigateToWaypointsX> - - >reactions; - - // STATE FUNCTIONS - static void staticConfigure() - { - configure_orthogonal(/*30*/ -180); - configure_orthogonal(); - configure_orthogonal(); - configure_orthogonal(); - } -}; -} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_rotate_degrees_5.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_rotate_degrees_5.hpp deleted file mode 100644 index 9d2c0d57b..000000000 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_rotate_degrees_5.hpp +++ /dev/null @@ -1,48 +0,0 @@ -// 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 -namespace sm_dancebot_ue -{ -// STATE DECLARATION -struct StRotateDegrees5 : smacc2::SmaccState -{ - using SmaccState::SmaccState; - - // TRANSITION TABLE - typedef mpl::list< - - Transition, StNavigateToWaypointsX>, - Transition, StNavigateToWaypointsX> - - >reactions; - - // STATE FUNCTIONS - static void staticConfigure() - { - configure_orthogonal(/*30*/ -180); - configure_orthogonal(); - configure_orthogonal(); - configure_orthogonal(); - } -}; -} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_rotate_degrees_6.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_rotate_degrees_6.hpp deleted file mode 100644 index 85ba0feed..000000000 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_rotate_degrees_6.hpp +++ /dev/null @@ -1,48 +0,0 @@ -// 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 -namespace sm_dancebot_ue -{ -// STATE DECLARATION -struct StRotateDegrees6 : smacc2::SmaccState -{ - using SmaccState::SmaccState; - - // TRANSITION TABLE - typedef mpl::list< - - Transition, StNavigateReverse3>, - Transition, StNavigateToWaypointsX> - - >reactions; - - // STATE FUNCTIONS - static void staticConfigure() - { - configure_orthogonal(/*30*/ -180); - configure_orthogonal(); - configure_orthogonal(); - configure_orthogonal(); - } -}; -} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_f_pattern_1.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_f_pattern_1.hpp index eefc25161..3879606ac 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_f_pattern_1.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_f_pattern_1.hpp @@ -56,7 +56,7 @@ struct SsFPattern1 // TRANSITION TABLE typedef mpl::list< - Transition>, StNavigateReverse4, ENDLOOP> //, + // Transition>, StNavigateReverse4, ENDLOOP> //, >reactions; @@ -73,7 +73,6 @@ struct SsFPattern1 // STATE FUNCTIONS static void staticConfigure() { - //configure_orthogonal(); } void runtimeConfigure() { iteration_count = 0; } diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_radial_pattern_1.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_radial_pattern_1.hpp index f8927c071..a0c3b2166 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_radial_pattern_1.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_radial_pattern_1.hpp @@ -49,7 +49,7 @@ struct SsRadialPattern1 typedef mpl::list< // Transition, StRotateDegrees1, ENDLOOP> - Transition, StNavigateReverse1, ENDLOOP> + // Transition, StNavigateReverse1, ENDLOOP> >reactions; @@ -62,7 +62,6 @@ struct SsRadialPattern1 // STATE FUNCTIONS static void staticConfigure() { - //configure_orthogonal(); } void runtimeConfigure() {} diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_radial_pattern_2.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_radial_pattern_2.hpp index ba57e72d9..6e573b351 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_radial_pattern_2.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_radial_pattern_2.hpp @@ -47,14 +47,13 @@ struct SsRadialPattern2 // TRANSITION TABLE typedef mpl::list< - Transition, StNavigateReverse1, ENDLOOP> + // Transition, StNavigateReverse1, ENDLOOP> >reactions; // STATE FUNCTIONS static void staticConfigure() { - //configure_orthogonal(); } void runtimeConfigure() {} diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_radial_pattern_3.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_radial_pattern_3.hpp index 2f1754f5a..026450d5e 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_radial_pattern_3.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_radial_pattern_3.hpp @@ -47,14 +47,13 @@ struct SsRadialPattern3 // TRANSITION TABLE typedef mpl::list< - Transition, StRotateDegrees4, ENDLOOP> + // Transition, StRotateDegrees4, ENDLOOP> >reactions; // STATE FUNCTIONS static void staticConfigure() { - //configure_orthogonal(); } int iteration_count; diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_s_pattern_1.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_s_pattern_1.hpp index 538d22b82..28f361349 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_s_pattern_1.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_s_pattern_1.hpp @@ -58,14 +58,13 @@ struct SsSPattern1 : smacc2::SmaccState, StNavigateReverse3, ENDLOOP> + // Transition, StNavigateReverse3, ENDLOOP> >reactions; // STATE FUNCTIONS static void staticConfigure() { - //configure_orthogonal(); } static constexpr float pitch1_lenght_meters() { return 0.75; } diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/package.xml b/smacc2_sm_reference_library/sm_dancebot_ue/package.xml index 9ee8642c2..e28d5a7fc 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/package.xml +++ b/smacc2_sm_reference_library/sm_dancebot_ue/package.xml @@ -26,6 +26,7 @@ sr_conditional visualization_msgs tf2 + ue_msgs backward_global_planner backward_local_planner diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan_back_on_road.yaml b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan_back_on_road.yaml new file mode 100644 index 000000000..ab88dab3c --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan_back_on_road.yaml @@ -0,0 +1,282 @@ +waypoints: + # POINT 36 + - position: + x: 288.55 + y: 637.19 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 37 + - position: + x: 286.85 + y: 606.79 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 38 + - position: + x: 277.15 + y: 569.29 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 39 + - position: + x: 262.85 + y: 535.19 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 40 + - position: + x: 240.75 + y: 494.89 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 41 + - position: + x: 214.65 + y: 453.99 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 42 + - position: + x: 185.95 + y: 404.89 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 43 + - position: + x: 160.45 + y: 354.09 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 44 + - position: + x: 138.55 + y: 303.89 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 45 + - position: + x: 118.05 + y: 258.59 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 46 + - position: + x: 98.05 + y: 213.19 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 47 + - position: + x: 83.45 + y: 182.19 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 48 + - position: + x: 70.55 + y: 149.99 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 49 + - position: + x: 58.65 + y: 124.49 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 50 + - position: + x: 46.35 + y: 97.49 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 51 + - position: + x: 36.55 + y: 76.29 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 52 + - position: + x: 26.65 + y: 52.79 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 53 + - position: + x: 16.25 + y: 28.39 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 54 + - position: + x: 3.65 + y: 6.09 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 55 + - position: + x: -0.764 + y: -11.90 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.7071068 + w: 0.7071068 + # POINT 56 + - position: + x: -28.14 + y: -40.50 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 57 + - position: + x: -43.44 + y: -62.50 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 58 + - position: + x: -56.44 + y: -81.30 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 59 + - position: + x: -65.84 + y: -102.20 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 60 + - position: + x: -77.94 + y: -132.00 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 61 + - position: + x: -86.14 + y: -152.20 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 62 + - position: + x: -92.94 + y: -179.10 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 63 + - position: + x: -94.64 + y: -206.00 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.7071068 + w: 0.7071068 + diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan_initial_road.yaml b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan_initial_road.yaml new file mode 100644 index 000000000..6179ff5e0 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan_initial_road.yaml @@ -0,0 +1,251 @@ +waypoints: + # POINT 0 + - position: + x: -94.64 + y: -206.00 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 0.0 + # POINT 1 + - position: + x: -92.94 + y: -179.10 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 2 + - position: + x: -86.14 + y: -152.20 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 3 + - position: + x: -77.94 + y: -132.00 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 4 + - position: + x: -65.84 + y: -102.20 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 5 + - position: + x: -56.44 + y: -81.30 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 6 + - position: + x: -43.44 + y: -62.50 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 7 + - position: + x: -28.14 + y: -40.50 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 8 + - position: + x: -7.64 + y: -11.90 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.7071068 + w: 0.7071068 + # POINT 9 + - position: + x: 3.65 + y: 6.09 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 10 + - position: + x: 16.25 + y: 28.39 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 11 + - position: + x: 26.65 + y: 52.79 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 12 + - position: + x: 36.55 + y: 76.29 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 13 + - position: + x: 46.35 + y: 97.49 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 14 + - position: + x: 58.65 + y: 124.49 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.7071068 + w: 0.7071068 + # POINT 15 + - position: + x: 70.55 + y: 149.99 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 16 + - position: + x: 83.45 + y: 182.19 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 17 + - position: + x: 98.05 + y: 213.19 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 18 + - position: + x: 118.05 + y: 258.59 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.7071068 + w: 0.7071068 + # POINT 19 + - position: + x: 138.55 + y: 303.89 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 20 + - position: + x: 160.45 + y: 354.09 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 21 + - position: + x: 214.65 + y: 453.99 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 22 + - position: + x: 240.75 + y: 494.89 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 23 + - position: + x: 262.85 + y: 535.19 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 24 + - position: + x: 277.15 + y: 569.29 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan_navigate_field.yaml b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan_navigate_field.yaml new file mode 100644 index 000000000..b63b39bcf --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan_navigate_field.yaml @@ -0,0 +1,111 @@ +waypoints: + # POINT 25 + - position: + x: 284.55 + y: 573.89 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 26 + - position: + x: 293.25 + y: 575.39 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 27 + - position: + x: 304.35 + y: 574.09 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 28 + - position: + x: 333.65 + y: 575.79 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 29 + - position: + x: 344.55 + y: 579.19 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 30 + - position: + x: 351.65 + y: 587.99 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 31 + - position: + x: 363.75 + y: 611.69 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 32 + - position: + x: 370.65 + y: 626.49 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 33 + - position: + x: 367.55 + y: 631.49 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 34 + - position: + x: 354.75 + y: 632.69 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 35 + - position: + x: 335.05 + y: 620.59 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/params/sm_dance_bot_config.yaml b/smacc2_sm_reference_library/sm_dancebot_ue/params/sm_dance_bot_config.yaml deleted file mode 100644 index 977ed0301..000000000 --- a/smacc2_sm_reference_library/sm_dancebot_ue/params/sm_dance_bot_config.yaml +++ /dev/null @@ -1,9 +0,0 @@ -SmDanceBot: - ros__parameters: - use_sim_time: true - signal_detector_loop_freq: 20.0 - clear_angular_distance_threshold: 0.1 # 0.05 - clear_point_distance_threshold: 0.4 #0.1 - record_angular_distance_threshold: 0.005 - record_point_distance_threshold: 0.1 - waypoints_plan: $(pkg_share)/params/nav2z_client/waypoints_plan.yaml diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/params/sm_dancebot_ue_config.yaml b/smacc2_sm_reference_library/sm_dancebot_ue/params/sm_dancebot_ue_config.yaml new file mode 100644 index 000000000..4b5e6ee54 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/params/sm_dancebot_ue_config.yaml @@ -0,0 +1,12 @@ +SmDanceBot: + ros__parameters: + use_sim_time: true + signal_detector_loop_freq: 20.0 + clear_angular_distance_threshold: 0.1 # 0.05 + clear_point_distance_threshold: 0.4 #0.1 + record_angular_distance_threshold: 0.005 + record_point_distance_threshold: 0.1 + + waypoints_plan_initial_road: $(pkg_share)/params/nav2z_client/waypoints_plan_initial_road.yaml + waypoints_plan_navigate_field: $(pkg_share)/params/nav2z_client/waypoints_plan_navigate_field.yaml + waypoints_plan_back_on_road: $(pkg_share)/params/nav2z_client/waypoints_plan_back_on_road.yaml \ No newline at end of file diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/clients/cl_led/cl_led.cpp b/smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/clients/cl_led/cl_led.cpp deleted file mode 100644 index 9725f82eb..000000000 --- a/smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/clients/cl_led/cl_led.cpp +++ /dev/null @@ -1,46 +0,0 @@ -// 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 - * - ******************************************************************************************************************/ -#include -//#include - -namespace sm_dancebot_ue -{ -namespace cl_led -{ -ClLED::ClLED(std::string actionServerName) -: SmaccActionClientBase(actionServerName) -{ -} - -std::string ClLED::getName() const { return "TOOL ACTION CLIENT"; } - -ClLED::~ClLED() {} - -std::ostream & operator<<( - std::ostream & out, const sm_dancebot_ue::action::LEDControl::Goal & msg) -{ - out << "LED CONTROL: " << msg.command; - return out; -} - -} // namespace cl_led - -//PLUGINLIB_EXPORT_CLASS(cl_led::ClLED, smacc2::ISmaccComponent) -} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/clients/client_behaviors/nav2z_client/cb_position_control_free_space.cpp b/smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/clients/client_behaviors/nav2z_client/cb_position_control_free_space.cpp new file mode 100644 index 000000000..058141500 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/clients/client_behaviors/nav2z_client/cb_position_control_free_space.cpp @@ -0,0 +1,205 @@ +// 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 + * + ******************************************************************************************************************/ + +#include +#include +#include + +#include +#include + +namespace sm_dancebot_ue +{ +CbPositionControlFreeSpace::CbPositionControlFreeSpace() +: targetYaw_(0), k_betta_(1.0), max_angular_yaw_speed_(1.0), yaw_goal_tolerance_rads_(0.03) +{ + // Set the target pose (modify this according to your desired goal) + // target_pose_.position.x = 3.0; + // target_pose_.position.y = 2.0; + // target_pose_.orientation.w = 1.0; // No rotation in this example + + // Set the threshold distance for goal achievement (modify this according to your needs) + threshold_distance_ = 2.0; // 10 centimeters +} + +void CbPositionControlFreeSpace::updateParameters() {} + +void CbPositionControlFreeSpace::onEntry() +{ + auto nh = this->getNode(); + cmd_vel_pub_ = nh->create_publisher("/cmd_vel", rclcpp::QoS(1)); + + sm_dancebot_ue::CpUEPose * pose; + this->requiresComponent(pose); + + geometry_msgs::msg::Twist cmd_vel; + goalReached_ = false; + + geometry_msgs::msg::Pose currentPose = pose->toPoseMsg(); + + rclcpp::Rate loop_rate(10); + double countAngle = 0; + auto prevyaw = tf2::getYaw(currentPose.orientation); + + // PID controller gains (proportional, integral, and derivative) + double kp_linear = 0.5; + double ki_linear = 0.0; + double kd_linear = 0.1; + + double kp_angular = 0.5; + double ki_angular = 0.0; + double kd_angular = 0.1; + + while (rclcpp::ok() && !goalReached_) + { + RCLCPP_INFO_STREAM_THROTTLE( + getLogger(), *getNode()->get_clock(), 200, + "CbPositionControlFreeSpace, current pose: " << currentPose.position.x << ", " + << currentPose.position.y << ", " + << tf2::getYaw(currentPose.orientation)); + + RCLCPP_INFO_STREAM_THROTTLE( + getLogger(), *getNode()->get_clock(), 200, + "CbPositionControlFreeSpace, target pose: " << target_pose_.position.x << ", " + << target_pose_.position.y << ", " + << tf2::getYaw(target_pose_.orientation)); + + tf2::Quaternion q; + currentPose = pose->toPoseMsg(); + + // Here we implement the control logic to calculate the velocity command + // based on the received position of the vehicle and the target pose. + + // Calculate the errors in x and y + double error_x = target_pose_.position.x - currentPose.position.x; + double error_y = target_pose_.position.y - currentPose.position.y; + + // Calculate the distance to the target pose + double distance_to_target = std::sqrt(error_x * error_x + error_y * error_y); + + RCLCPP_INFO_STREAM( + getLogger(), "[" << getName() << "] distance to target: " << distance_to_target + << " ( th: " << threshold_distance_ << ")"); + + // Check if the robot has reached the target pose + if (distance_to_target < threshold_distance_) + { + RCLCPP_INFO(getLogger(), "Goal reached!"); + // Stop the robot by setting the velocity commands to zero + geometry_msgs::msg::Twist cmd_vel_msg; + cmd_vel_msg.linear.x = 0.0; + cmd_vel_msg.angular.z = 0.0; + cmd_vel_pub_->publish(cmd_vel_msg); + break; + } + else + { + // Calculate the desired orientation angle + double desired_yaw = std::atan2(error_y, error_x); + + // Calculate the difference between the desired orientation and the current orientation + double yaw_error = desired_yaw - (tf2::getYaw(currentPose.orientation) + M_PI ); + + // Ensure the yaw error is within the range [-pi, pi] + while (yaw_error > M_PI) yaw_error -= 2 * M_PI; + while (yaw_error < -M_PI) yaw_error += 2 * M_PI; + + // Calculate the control signals (velocity commands) using PID controllers + double cmd_linear_x = kp_linear * distance_to_target + ki_linear * integral_linear_ + + kd_linear * (distance_to_target - prev_error_linear_); + double cmd_angular_z = kp_angular * yaw_error + ki_angular * integral_angular_ + + kd_angular * (yaw_error - prev_error_angular_); + + // Limit the maximum linear velocity and angular velocity to avoid sudden movements + double max_linear_velocity = 0.5; // Adjust this value according to your needs + double max_angular_velocity = 1.0; // Adjust this value according to your needs + + if (cmd_linear_x > max_linear_velocity) + cmd_linear_x = max_linear_velocity; + else if (cmd_linear_x < -max_linear_velocity) + cmd_linear_x = -max_linear_velocity; + + if (cmd_angular_z > max_angular_velocity) + cmd_angular_z = max_angular_velocity; + else if (cmd_angular_z < -max_angular_velocity) + cmd_angular_z = -max_angular_velocity; + + // Construct and publish the velocity command message + geometry_msgs::msg::Twist cmd_vel_msg; + + cmd_vel_msg.linear.x = cmd_linear_x; + cmd_vel_msg.angular.z = cmd_angular_z; + + cmd_vel_pub_->publish(cmd_vel_msg); + + // Update errors and integrals for the next control cycle + prev_error_linear_ = distance_to_target; + prev_error_angular_ = yaw_error; + integral_linear_ += distance_to_target; + integral_angular_ += yaw_error; + + // tf2::fromMsg(currentPose.orientation, q); + // auto currentYaw = tf2::getYaw(currentPose.orientation); + // auto deltaAngle = angles::shortest_angular_distance(prevyaw, currentYaw); + // countAngle += deltaAngle; + + // prevyaw = currentYaw; + // double angular_error = targetYaw_ - countAngle; + + // auto omega = angular_error * k_betta_; + // cmd_vel.linear.x = 0; + // cmd_vel.linear.y = 0; + // cmd_vel.linear.z = 0; + // cmd_vel.angular.z = + // std::min(std::max(omega, -fabs(max_angular_yaw_speed_)), fabs(max_angular_yaw_speed_)); + + // RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] delta angle: " << deltaAngle); + // RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] cummulated angle: " << countAngle); + // RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] k_betta_: " << k_betta_); + + // RCLCPP_INFO_STREAM( + // getLogger(), "[" << getName() << "] angular error: " << angular_error << "(" + // << yaw_goal_tolerance_rads_ << ")"); + // RCLCPP_INFO_STREAM( + // getLogger(), "[" << getName() << "] command angular speed: " << cmd_vel.angular.z); + + // if (fabs(angular_error) < yaw_goal_tolerance_rads_) + // { + // RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] GOAL REACHED. Sending stop command."); + // goalReached_ = true; + // cmd_vel.linear.x = 0; + // cmd_vel.angular.z = 0; + // break; + // } + + // this->cmd_vel_pub_->publish(cmd_vel); + + loop_rate.sleep(); + } + } + + RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] Finished behavior execution"); + + this->postSuccessEvent(); +} + +void CbPositionControlFreeSpace::onExit() {} + +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/clients/components/cp_ue_pose.cpp b/smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/clients/components/cp_ue_pose.cpp new file mode 100644 index 000000000..4c25fba0f --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/clients/components/cp_ue_pose.cpp @@ -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. + +/***************************************************************************************************************** + *-2020 + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace sm_dancebot_ue +{ +CpUEPose::CpUEPose(std::string topicname) + : CpTopicSubscriber(topicname) +{ +} + +void CpUEPose::onInitialize() +{ + CpTopicSubscriber::onInitialize(); + this->onMessageReceived(&CpUEPose::onPoseMessageReceived, this); +} + +void CpUEPose::onPoseMessageReceived(const ue_msgs::msg::EntityState& msg) +{ + this->entityStateMsg_ = msg;; + + RCLCPP_INFO_STREAM_THROTTLE(getLogger(), *(getNode()->get_clock()), 200, "Received UEPose x: " << msg.pose.position.x << " y: " << msg.pose.position.y << " z: " << msg.pose.position.z); +} + +geometry_msgs::msg::Pose CpUEPose::toPoseMsg() +{ + return this->entityStateMsg_.pose; +} + +} // namespace cl_nav2z From 525da3fc921d29dd82cf63fc2fddf12c90e1ef82 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pablo=20I=C3=B1igo=20Blasco?= Date: Fri, 4 Aug 2023 19:53:32 +0200 Subject: [PATCH 48/70] turn over state machine --- .../cp_waypoints_navigator.hpp | 4 + .../cp_waypoints_navigator_base.hpp | 7 +- .../cp_waypoints_navigator.cpp | 12 ++ .../sm_dance_bot/CHANGELOG.rst | 5 + .../states/st_event_count_down.hpp | 58 ++++++++ .../sm_dance_bot/launch/bringup_launch.py | 4 +- .../sm_dance_bot/launch/gazebo_launch.py | 3 +- .../sm_dance_bot/package.xml | 2 +- .../sm_dancebot_ue/CMakeLists.txt | 1 + .../client_behaviors/cb_active_stop.hpp | 46 +++++++ .../cb_navigate_next_waypoint_free.hpp | 2 +- .../cb_position_control_free_space.hpp | 12 +- .../client_behaviors/cb_pure_spinning.hpp | 126 ++++++++++++++++++ .../orthogonals/or_navigation.hpp | 2 +- .../include/sm_dancebot_ue/sm_dancebot_ue.hpp | 21 ++- .../states/st_acquire_sensors.hpp | 1 + .../states/st_back_on_road_waypoints_x.hpp | 3 +- ...vent_count_down.hpp => st_final_state.hpp} | 15 ++- .../states/st_inital_road_waypoints_x.hpp | 9 +- .../states/st_navigate_field_waypoints_x.hpp | 6 +- .../sm_dancebot_ue/states/st_turn_around.hpp | 54 ++++++++ .../waypoints_plan_back_on_road.yaml | 1 - .../params/sm_dancebot_ue_config.yaml | 2 +- .../nav2z_client/cb_active_stop.cpp | 53 ++++++++ .../cb_position_control_free_space.cpp | 12 +- 25 files changed, 406 insertions(+), 55 deletions(-) create mode 100644 smacc2_sm_reference_library/sm_dance_bot/include/sm_dance_bot/states/st_event_count_down.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_active_stop.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_pure_spinning.hpp rename smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/{st_event_count_down.hpp => st_final_state.hpp} (79%) create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_turn_around.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/clients/client_behaviors/nav2z_client/cb_active_stop.cpp diff --git a/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.hpp b/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.hpp index 37ccf51e5..4e71561be 100644 --- a/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.hpp +++ b/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.hpp @@ -30,6 +30,10 @@ namespace cl_nav2z { class ClNav2Z; +struct EvWaypointFinal : sc::event +{ +}; + struct NavigateNextWaypointOptions { std::optional controllerName_; diff --git a/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator_base.hpp b/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator_base.hpp index 3726d147e..cb6b12465 100644 --- a/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator_base.hpp +++ b/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator_base.hpp @@ -27,7 +27,6 @@ namespace cl_nav2z { - struct Pose2D { Pose2D(double x, double y, double yaw) @@ -76,7 +75,6 @@ class CpWaypointNavigatorBase : public smacc2::ISmaccComponent geometry_msgs::msg::Pose getPose(int index) const; geometry_msgs::msg::Pose getCurrentPose() const; - long getCurrentWaypointIndex() const; std::optional getCurrentWaypointName() const; @@ -87,7 +85,10 @@ class CpWaypointNavigatorBase : public smacc2::ISmaccComponent void forward(int count); void seekName(std::string name); - void loadWaypointsFromYamlParameter(std::string parameter_name, std::string yaml_file_package_name); + void loadWaypointsFromYamlParameter( + std::string parameter_name, std::string yaml_file_package_name); + + void notifyGoalReached(); protected: void insertWaypoint(int index, geometry_msgs::msg::Pose & newpose); diff --git a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.cpp b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.cpp index 9d26ff96e..d3163961a 100644 --- a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.cpp +++ b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.cpp @@ -68,6 +68,8 @@ void CpWaypointNavigator::onGoalReached(const ClNav2Z::WrappedResult & /*res*/) currentWaypoint_); stopWaitingResult(); + this->notifyGoalReached(); + onNavigationRequestSucceded(); } @@ -285,6 +287,16 @@ CpWaypointNavigator::sendNextGoal( return std::nullopt; } +void CpWaypointNavigatorBase::notifyGoalReached() +{ + // when it is the last waypoint post an finalization EOF event + if (currentWaypoint_ == (long)waypoints_.size() - 1) + { + RCLCPP_WARN(getLogger(), "[CpWaypointNavigator] Last waypoint reached, posting EOF event. "); + this->postEvent(); + } +} + void CpWaypointNavigator::onNavigationResult(const ClNav2Z::WrappedResult & r) { if (r.code == rclcpp_action::ResultCode::SUCCEEDED) diff --git a/smacc2_sm_reference_library/sm_dance_bot/CHANGELOG.rst b/smacc2_sm_reference_library/sm_dance_bot/CHANGELOG.rst index 89f28db94..4d78731bd 100644 --- a/smacc2_sm_reference_library/sm_dance_bot/CHANGELOG.rst +++ b/smacc2_sm_reference_library/sm_dance_bot/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package sm_dance_bot ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.3.16 (2023-07-16) +------------------- +* minor +* Contributors: pabloinigoblasco + 2.3.6 (2023-03-12) ------------------ diff --git a/smacc2_sm_reference_library/sm_dance_bot/include/sm_dance_bot/states/st_event_count_down.hpp b/smacc2_sm_reference_library/sm_dance_bot/include/sm_dance_bot/states/st_event_count_down.hpp new file mode 100644 index 000000000..f7b9adf5a --- /dev/null +++ b/smacc2_sm_reference_library/sm_dance_bot/include/sm_dance_bot/states/st_event_count_down.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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#pragma once + +#include + +namespace sm_dance_bot +{ +// STATE DECLARATION +struct StEventCountDown : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StNavigateToWaypointsX>, + Transition + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + // configure_orthogonal(); + // configure_orthogonal("Hello World!"); + // configure_orthogonal(); + // configure_orthogonal(Service3Command::SERVICE3_ON); + + // Create State Reactor + + //auto srCountdown = static_createStateReactor(5); + //srCountdown->addInputEvent>(); + //srCountdown->setOutputEvent>(); + + auto srCountdown = static_createStateReactor< + SrEventCountdown, EvCountdownEnd, mpl::list>>(5); + } +}; +} // namespace sm_dance_bot diff --git a/smacc2_sm_reference_library/sm_dance_bot/launch/bringup_launch.py b/smacc2_sm_reference_library/sm_dance_bot/launch/bringup_launch.py index aa25a3f78..627d65536 100644 --- a/smacc2_sm_reference_library/sm_dance_bot/launch/bringup_launch.py +++ b/smacc2_sm_reference_library/sm_dance_bot/launch/bringup_launch.py @@ -25,8 +25,8 @@ ) from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration +from launch.substitutions import LaunchConfiguration, PythonExpression from launch_ros.actions import PushRosNamespace @@ -39,7 +39,7 @@ def generate_launch_description(): namespace = LaunchConfiguration("namespace") use_namespace = LaunchConfiguration("use_namespace") slam = LaunchConfiguration("slam") - # map_yaml_file = LaunchConfiguration("map") + map_yaml_file = LaunchConfiguration("map") use_sim_time = LaunchConfiguration("use_sim_time") params_file = LaunchConfiguration("params_file") default_nav_to_pose_bt_xml = LaunchConfiguration("default_nav_to_pose_bt_xml") diff --git a/smacc2_sm_reference_library/sm_dance_bot/launch/gazebo_launch.py b/smacc2_sm_reference_library/sm_dance_bot/launch/gazebo_launch.py index 68fdc89fc..90692960a 100644 --- a/smacc2_sm_reference_library/sm_dance_bot/launch/gazebo_launch.py +++ b/smacc2_sm_reference_library/sm_dance_bot/launch/gazebo_launch.py @@ -19,7 +19,8 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, ExecuteProcess from launch.conditions import IfCondition, UnlessCondition -from launch.substitutions import LaunchConfiguration + +from launch.substitutions import LaunchConfiguration, PythonExpression def generate_launch_description(): diff --git a/smacc2_sm_reference_library/sm_dance_bot/package.xml b/smacc2_sm_reference_library/sm_dance_bot/package.xml index f8a8fca71..d906d31fe 100644 --- a/smacc2_sm_reference_library/sm_dance_bot/package.xml +++ b/smacc2_sm_reference_library/sm_dance_bot/package.xml @@ -2,7 +2,7 @@ sm_dance_bot - 2.2.2 + 2.3.18 The dance_bot package Pablo Inigo Blasco diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/CMakeLists.txt b/smacc2_sm_reference_library/sm_dancebot_ue/CMakeLists.txt index e99db0db0..1d16bacb2 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/CMakeLists.txt +++ b/smacc2_sm_reference_library/sm_dancebot_ue/CMakeLists.txt @@ -58,6 +58,7 @@ add_executable(${PROJECT_NAME}_node src/${PROJECT_NAME}/sm_dancebot_ue.cpp src/${PROJECT_NAME}/clients/client_behaviors/nav2z_client/cb_position_control_free_space.cpp src/${PROJECT_NAME}/clients/components/cp_ue_pose.cpp + src/${PROJECT_NAME}/clients/client_behaviors/nav2z_client/cb_active_stop.cpp ) add_executable(temperature_sensor_node_${PROJECT_NAME} servers/temperature_sensor_node/src/temperature_sensor_node.cpp) diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_active_stop.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_active_stop.hpp new file mode 100644 index 000000000..5f7e71773 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_active_stop.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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include + +namespace sm_dancebot_ue +{ +struct CbActiveStop : public smacc2::SmaccAsyncClientBehavior +{ +private: + + + rclcpp::Publisher::SharedPtr cmd_vel_pub_; + +public: + + CbActiveStop(); + + void onEntry() override; + + void onExit() override; +}; +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_navigate_next_waypoint_free.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_navigate_next_waypoint_free.hpp index 7a39cedf1..919f89446 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_navigate_next_waypoint_free.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_navigate_next_waypoint_free.hpp @@ -39,12 +39,12 @@ class CbNavigateNextWaypointFree : public sm_dancebot_ue::CbPositionControlFreeS this->onSuccess(&CbNavigateNextWaypointFree::onSucessCallback, this); RCLCPP_INFO_STREAM(getLogger(), "[CbNavigateNextWaypoint] initial load file target pose: x: " << this->target_pose_.position.x << ", y: " << this->target_pose_.position.y); CbPositionControlFreeSpace::onEntry(); - } void onSucessCallback() { RCLCPP_INFO_STREAM(getLogger(), "[CbNavigateNextWaypoint] Success on planning to next waypoint"); + this->waypointsNavigator_->notifyGoalReached(); this->waypointsNavigator_->forward(1); RCLCPP_INFO_STREAM(getLogger(), "[CbNavigateNextWaypoint] next position index: " << this->waypointsNavigator_->getCurrentWaypointIndex() << "/" << this->waypointsNavigator_->getWaypoints().size()); } diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_position_control_free_space.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_position_control_free_space.hpp index f463ae303..c7c824f0e 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_position_control_free_space.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_position_control_free_space.hpp @@ -18,7 +18,7 @@ * ******************************************************************************************************************/ -#pragma once +#pragma once #include #include @@ -36,16 +36,21 @@ struct CbPositionControlFreeSpace : public smacc2::SmaccAsyncClientBehavior double k_betta_; double max_angular_yaw_speed_; - double threshold_distance_; double prev_error_linear_ = 0.0; double prev_error_angular_ = 0.0; double integral_linear_ = 0.0; double integral_angular_ = 0.0; + // Limit the maximum linear velocity and angular velocity to avoid sudden movements + double max_linear_velocity = 1.0; // Adjust this value according to your needs + double max_angular_velocity = 1.0; // Adjust this value according to your needs + rclcpp::Publisher::SharedPtr cmd_vel_pub_; public: - double yaw_goal_tolerance_rads_; + double yaw_goal_tolerance_rads_=0.1; + + double threshold_distance_ = 3.0; geometry_msgs::msg::Pose target_pose_; @@ -56,6 +61,5 @@ struct CbPositionControlFreeSpace : public smacc2::SmaccAsyncClientBehavior void onEntry() override; void onExit() override; - }; } // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_pure_spinning.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_pure_spinning.hpp new file mode 100644 index 000000000..925c9334b --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_pure_spinning.hpp @@ -0,0 +1,126 @@ +// 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 + +namespace sm_dancebot_ue +{ +struct CbPureSpinning : public smacc2::SmaccAsyncClientBehavior +{ + private: + double targetYaw__rads; + bool goalReached_; + double k_betta_; + double max_angular_yaw_speed_; + + rclcpp::Publisher::SharedPtr cmd_vel_pub_; + + public: + double yaw_goal_tolerance_rads_; + + CbPureSpinning(double targetYaw_rads, double max_angular_yaw_speed= 0.5) + : targetYaw__rads(targetYaw_rads), + k_betta_(1.0), + max_angular_yaw_speed_(max_angular_yaw_speed), + yaw_goal_tolerance_rads_(0.1) + { + + } + + void updateParameters() + { + } + + void onEntry() override + { + auto nh = this->getNode(); + cmd_vel_pub_ = nh->create_publisher("/cmd_vel", rclcpp::QoS(1)); + + sm_dancebot_ue::CpUEPose* pose; + this->requiresComponent(pose); + + geometry_msgs::msg::Twist cmd_vel; + goalReached_ = false; + + geometry_msgs::msg::Pose currentPose = pose->toPoseMsg(); + + rclcpp::Rate loop_rate(10); + double countAngle = 0; + auto prevyaw = tf2::getYaw(currentPose.orientation); + while(rclcpp::ok() && !goalReached_) + { + tf2::Quaternion q; + currentPose = pose->toPoseMsg(); + tf2::fromMsg(currentPose.orientation, q); + auto currentYaw = tf2::getYaw(currentPose.orientation); + auto deltaAngle = angles::shortest_angular_distance(prevyaw, currentYaw); + countAngle += deltaAngle; + + prevyaw = currentYaw; + double angular_error = targetYaw__rads - countAngle ; + + auto omega = angular_error * k_betta_; + cmd_vel.linear.x = 0; + cmd_vel.linear.y = 0; + cmd_vel.linear.z = 0; + cmd_vel.angular.z = + std::min(std::max(omega, -fabs(max_angular_yaw_speed_)), fabs(max_angular_yaw_speed_)); + + RCLCPP_INFO_STREAM(getLogger(), "["<cmd_vel_pub_->publish(cmd_vel); + + loop_rate.sleep(); + } + + this->postSuccessEvent(); + } + + void onExit() override + { + } + }; +} \ No newline at end of file diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_navigation.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_navigation.hpp index 80e4d5067..9e735ea56 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_navigation.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_navigation.hpp @@ -39,7 +39,7 @@ class OrNavigation : public smacc2::Orthogonal nav2zClient->createComponent("/ue_ros/map_origin_entity_state"); - auto waypointsNavigator = nav2zClient->createComponent<::cl_nav2z::CpWaypointNavigatorBase>(); + /*auto waypointsNavigator = */nav2zClient->createComponent<::cl_nav2z::CpWaypointNavigatorBase>(); } }; } // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/sm_dancebot_ue.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/sm_dancebot_ue.hpp index 86c5daf0d..2f057c941 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/sm_dancebot_ue.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/sm_dancebot_ue.hpp @@ -33,8 +33,8 @@ #include #include #include - -using namespace cl_nav2z; +#include +#include #include #include @@ -47,21 +47,23 @@ using namespace cl_nav2z; #include #include -using namespace smacc2::state_reactors; // ORTHOGONALS #include #include +using namespace cl_nav2z; +using namespace smacc2::state_reactors; + namespace sm_dancebot_ue { //STATE FORWARD DECLARATIONS class StAcquireSensors; -class StEventCountDown; - class StInitialRoadWaypointsX; class StNavigateFieldWaypointsX; class StBackOnRoadWaypointsX; +class StTurnAround; +class StFinalState; //SUPERSTATE FORWARD DECLARATIONS //MODE STATES FORWARD DECLARATIONS @@ -110,9 +112,6 @@ namespace sm_dancebot_ue /// for the development of state machines struct SmDanceBot : public smacc2::SmaccStateMachineBase { - int counter_1; - bool rt_ready_flag; - typedef mpl::bool_ shallow_history; typedef mpl::bool_ deep_history; typedef mpl::bool_ inherited_deep_history; @@ -121,9 +120,6 @@ struct SmDanceBot : public smacc2::SmaccStateMachineBasesetGlobalSMData("counter_1", counter_1); - this->setGlobalSMData("rt_ready_flag", rt_ready_flag); - this->createOrthogonal(); this->createOrthogonal(); } @@ -145,8 +141,9 @@ struct SmDanceBot : public smacc2::SmaccStateMachineBase -#include #include #include #include +#include +#include diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_acquire_sensors.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_acquire_sensors.hpp index 246a09df1..0f1e08264 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_acquire_sensors.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_acquire_sensors.hpp @@ -40,6 +40,7 @@ struct StAcquireSensors : smacc2::SmaccState, StInitialRoadWaypointsX, ON_SENSORS_AVAILABLE>, Transition diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_back_on_road_waypoints_x.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_back_on_road_waypoints_x.hpp index 81cad34c0..563031195 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_back_on_road_waypoints_x.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_back_on_road_waypoints_x.hpp @@ -40,17 +40,18 @@ struct StBackOnRoadWaypointsX : smacc2::SmaccState, Transition, StBackOnRoadWaypointsX, TRANSITION_1> // Transition, StNavigateToWaypointsX, TRANSITION_2>, // Transition, SS1::SsRadialPattern1, TRANSITION_3>, // Transition, SS2::SsRadialPattern2, TRANSITION_4> - >reactions; // STATE FUNCTIONS static void staticConfigure() { // configure_orthogonal(); + configure_orthogonal("waypoints_plan_back_on_road", "sm_dancebot_ue"); configure_orthogonal(); } diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_event_count_down.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_final_state.hpp similarity index 79% rename from smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_event_count_down.hpp rename to smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_final_state.hpp index 328b03ff3..fb92c19e3 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_event_count_down.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_final_state.hpp @@ -24,23 +24,28 @@ namespace sm_dancebot_ue { +using namespace smacc2::default_events; +using smacc2::client_behaviors::CbSleepFor; +using namespace std::chrono_literals; + // STATE DECLARATION -struct StEventCountDown : smacc2::SmaccState +struct StFinalState : smacc2::SmaccState { using SmaccState::SmaccState; // TRANSITION TABLE typedef mpl::list< - - // Transition, StNavigateToWaypointsX>, - Transition - >reactions; // STATE FUNCTIONS static void staticConfigure() { + configure_orthogonal(); + } + void runtimeConfigure() + { + } }; } // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_inital_road_waypoints_x.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_inital_road_waypoints_x.hpp index 9128cd17f..e5dec75dc 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_inital_road_waypoints_x.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_inital_road_waypoints_x.hpp @@ -40,17 +40,14 @@ struct StInitialRoadWaypointsX : smacc2::SmaccState, StInitialRoadWaypointsX, TRANSITION_1> - // Transition, StNavigateToWaypointsX, TRANSITION_2>, - // Transition, SS1::SsRadialPattern1, TRANSITION_3>, - // Transition, SS2::SsRadialPattern2, TRANSITION_4> - + Transition, + //Transition, StTurnAround, TRANSITION_1>, + Transition, StInitialRoadWaypointsX, TRANSITION_2> >reactions; // STATE FUNCTIONS static void staticConfigure() { - // configure_orthogonal(); configure_orthogonal(); } diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_field_waypoints_x.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_field_waypoints_x.hpp index 429aa7d5a..89556a636 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_field_waypoints_x.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_field_waypoints_x.hpp @@ -40,11 +40,7 @@ struct StNavigateFieldWaypointsX : smacc2::SmaccState, StNavigateFieldWaypointsX, TRANSITION_1> - // Transition, StNavigateToWaypointsX, TRANSITION_2>, - // Transition, SS1::SsRadialPattern1, TRANSITION_3>, - // Transition, SS2::SsRadialPattern2, TRANSITION_4> - + Transition, StNavigateFieldWaypointsX, TRANSITION_1> >reactions; // STATE FUNCTIONS diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_turn_around.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_turn_around.hpp new file mode 100644 index 000000000..683c29426 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_turn_around.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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#pragma once + +#include + +namespace sm_dancebot_ue +{ +using namespace smacc2::default_events; +using smacc2::client_behaviors::CbSleepFor; +using namespace std::chrono_literals; + +// STATE DECLARATION +struct StTurnAround : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + Transition, StBackOnRoadWaypointsX, SUCCESS> + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(M_PI, 0.8); + } + + void runtimeConfigure() + { + auto spinningBehavior = this->getOrthogonal()->getClientBehavior(); + spinningBehavior->yaw_goal_tolerance_rads_ = 0.2; + + } +}; +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan_back_on_road.yaml b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan_back_on_road.yaml index ab88dab3c..4dc8dbc48 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan_back_on_road.yaml +++ b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan_back_on_road.yaml @@ -279,4 +279,3 @@ waypoints: y: 0.0 z: 0.7071068 w: 0.7071068 - diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/params/sm_dancebot_ue_config.yaml b/smacc2_sm_reference_library/sm_dancebot_ue/params/sm_dancebot_ue_config.yaml index 4b5e6ee54..f53863a47 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/params/sm_dancebot_ue_config.yaml +++ b/smacc2_sm_reference_library/sm_dancebot_ue/params/sm_dancebot_ue_config.yaml @@ -9,4 +9,4 @@ SmDanceBot: waypoints_plan_initial_road: $(pkg_share)/params/nav2z_client/waypoints_plan_initial_road.yaml waypoints_plan_navigate_field: $(pkg_share)/params/nav2z_client/waypoints_plan_navigate_field.yaml - waypoints_plan_back_on_road: $(pkg_share)/params/nav2z_client/waypoints_plan_back_on_road.yaml \ No newline at end of file + waypoints_plan_back_on_road: $(pkg_share)/params/nav2z_client/waypoints_plan_back_on_road.yaml diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/clients/client_behaviors/nav2z_client/cb_active_stop.cpp b/smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/clients/client_behaviors/nav2z_client/cb_active_stop.cpp new file mode 100644 index 000000000..a912ea9d0 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/clients/client_behaviors/nav2z_client/cb_active_stop.cpp @@ -0,0 +1,53 @@ + +// 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 + * + ******************************************************************************************************************/ + +#include +#include +#include + +#include +#include + +namespace sm_dancebot_ue +{ +CbActiveStop::CbActiveStop() {} + +void CbActiveStop::onEntry() +{ + auto nh = this->getNode(); + cmd_vel_pub_ = nh->create_publisher("/cmd_vel", rclcpp::QoS(1)); + + rclcpp::Rate loop_rate(5); + geometry_msgs::msg::Twist cmd_vel_msg; + while (!this->isShutdownRequested()) + { + cmd_vel_msg.linear.x = 0; + cmd_vel_msg.angular.z = 0; + + cmd_vel_pub_->publish(cmd_vel_msg); + loop_rate.sleep(); + } + RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] Finished behavior execution"); + + this->postSuccessEvent(); +} + +void CbActiveStop::onExit() {} + +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/clients/client_behaviors/nav2z_client/cb_position_control_free_space.cpp b/smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/clients/client_behaviors/nav2z_client/cb_position_control_free_space.cpp index 058141500..daf272ecd 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/clients/client_behaviors/nav2z_client/cb_position_control_free_space.cpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/clients/client_behaviors/nav2z_client/cb_position_control_free_space.cpp @@ -28,15 +28,8 @@ namespace sm_dancebot_ue { CbPositionControlFreeSpace::CbPositionControlFreeSpace() -: targetYaw_(0), k_betta_(1.0), max_angular_yaw_speed_(1.0), yaw_goal_tolerance_rads_(0.03) +: targetYaw_(0), k_betta_(1.0), max_angular_yaw_speed_(1.0) { - // Set the target pose (modify this according to your desired goal) - // target_pose_.position.x = 3.0; - // target_pose_.position.y = 2.0; - // target_pose_.orientation.w = 1.0; // No rotation in this example - - // Set the threshold distance for goal achievement (modify this according to your needs) - threshold_distance_ = 2.0; // 10 centimeters } void CbPositionControlFreeSpace::updateParameters() {} @@ -127,9 +120,6 @@ void CbPositionControlFreeSpace::onEntry() double cmd_angular_z = kp_angular * yaw_error + ki_angular * integral_angular_ + kd_angular * (yaw_error - prev_error_angular_); - // Limit the maximum linear velocity and angular velocity to avoid sudden movements - double max_linear_velocity = 0.5; // Adjust this value according to your needs - double max_angular_velocity = 1.0; // Adjust this value according to your needs if (cmd_linear_x > max_linear_velocity) cmd_linear_x = max_linear_velocity; From 0f589845e48699a91edc8dc0dd3dae679e0865d0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pablo=20I=C3=B1igo=20Blasco?= Date: Wed, 9 Aug 2023 11:40:51 +0200 Subject: [PATCH 49/70] art gallery --- smacc2_dev_tools/.vscode/launch.json | 25 + .../sm_dancebot_artgallery_ue/CHANGELOG.rst | 13067 ++++++++++++++++ .../sm_dancebot_artgallery_ue/CMakeLists.txt | 113 + .../sm_dancebot_artgallery_ue/README.md | 55 + .../docker/.dockerignore | 0 .../docker/.gitignore | 1 + .../docker/Dockerfile | 131 + .../docker/README.md | 296 + .../docker/attach_docker_container.sh | 1 + .../docker/build_docker.sh | 21 + .../docker/build_docker_humble.sh | 4 + .../docker/fastdds_setup.bash | 5 + .../docker/frames_2023-08-05_01.06.53.gv | 18 + .../docker/frames_2023-08-05_01.06.53.pdf | Bin 0 -> 16806 bytes .../docker/join_bash.sh | 1 + .../docker/join_editor.sh | 1 + .../docker/nvidia-driver-check.sh | 62 + .../docker/openvpn/login.txt | 0 .../docker/openvpn/ovpnconfig.ovpn | 0 .../docker/openvpn/password.conf | 0 .../docker/openvpn_file_run.sh | 7 + .../docker/remove_container.sh | 1 + .../docker/run_docker_container_bash.sh | 8 + .../run_docker_container_bash_development.sh | 8 + .../docker/run_docker_container_editor.sh | 8 + .../docker/run_editor_smacc.sh | 32 + .../docker/save_current_docker_image.sh | 11 + .../docker/start_container.sh | 1 + .../docker/stop_container.sh | 1 + .../docker/update_project_files.sh | 2 + .../states/st_back_on_road_waypoints_x.hpp | 68 + .../states/st_inital_road_waypoints_x.hpp | 64 + .../client_behaviors/cb_active_stop.hpp | 46 + .../cb_load_waypoints_file.hpp | 63 + .../cb_navigate_next_waypoint_free.hpp | 60 + .../cb_position_control_free_space.hpp | 65 + .../client_behaviors/cb_pure_spinning.hpp | 126 + .../clients/components/cp_ue_pose.hpp | 56 + .../modestates/ms_dance_bot_recovery_mode.hpp | 38 + .../modestates/ms_dance_bot_run_mode.hpp | 37 + .../orthogonals/or_navigation.hpp | 71 + .../orthogonals/or_obstacle_perception.hpp | 39 + .../sm_dancebot_artgallery_ue.hpp | 149 + .../sti_fpattern_forward_1.hpp | 57 + .../sti_fpattern_forward_2.hpp | 51 + .../sti_fpattern_loop_start.hpp | 55 + .../sti_fpattern_return_1.hpp | 50 + .../sti_fpattern_rotate_1.hpp | 60 + .../sti_fpattern_rotate_2.hpp | 62 + .../sti_radial_end_point.hpp | 49 + .../sti_radial_loop_start.hpp | 59 + .../sti_radial_return.hpp | 55 + .../sti_radial_rotate.hpp | 57 + .../sti_spattern_forward_1.hpp | 48 + .../sti_spattern_forward_2.hpp | 48 + .../sti_spattern_forward_3.hpp | 46 + .../sti_spattern_forward_4.hpp | 50 + .../sti_spattern_loop_start.hpp | 60 + .../sti_spattern_rotate_1.hpp | 59 + .../sti_spattern_rotate_2.hpp | 61 + .../sti_spattern_rotate_3.hpp | 72 + .../sti_spattern_rotate_4.hpp | 60 + .../states/st_acquire_sensors.hpp | 73 + .../states/st_back_on_road_waypoints_x.hpp | 68 + .../states/st_final_state.hpp | 51 + .../states/st_inital_road_waypoints_x.hpp | 64 + .../states/st_navigate_field_waypoints_x.hpp | 63 + .../states/st_turn_around.hpp | 54 + .../superstates/ss_f_pattern_1.hpp | 91 + .../superstates/ss_radial_pattern_1.hpp | 77 + .../superstates/ss_radial_pattern_2.hpp | 80 + .../superstates/ss_radial_pattern_3.hpp | 75 + .../superstates/ss_s_pattern_1.hpp | 92 + .../launch/bringup_launch.py | 137 + .../launch/husky_gazebo.launch.yaml | 3 + .../launch/navigation_launch.py | 169 + .../launch/online_sync_launch.py | 61 + .../launch/rviz_launch.py | 122 + .../launch/slam_launch.py | 128 + .../sm_dancebot_artgallery_ue_launch.py | 267 + .../sm_dancebot_artgallery_ue/package.xml | 48 + .../params/mapper_params_online_sync.yaml | 73 + .../params/nav2z_client/nav2_params.yaml | 374 + .../params/nav2z_client/navigation_tree.xml | 33 + .../nav2z_client/waypoints_art_gallery.yaml | 141 + .../nav2z_client_husky/nav2_params.yaml | 367 + .../nav2z_client_husky/navigation_tree.xml | 27 + .../nav2z_client_husky/waypoints_plan.yaml | 82 + .../nav2z_client_turtlebot/nav2_params.yaml | 369 + .../navigation_tree.xml | 27 + .../waypoints_plan.yaml | 82 + .../sm_dancebot_artgallery_ue_config.yaml | 10 + .../rviz/nav2_default_view.rviz | 805 + .../rviz/nav2_namespaced_view.rviz | 371 + .../scripts/test_oscillation.py | 165 + .../scripts/transform_publisher.py | 118 + .../action/LEDControl.action | 18 + .../src/led_action_server_node.cpp | 230 + .../servers/service_node_3/service_node_3.py | 47 + .../src/temperature_sensor_node.cpp | 36 + .../nav2z_client/cb_active_stop.cpp | 53 + .../nav2z_client/cb_active_stop.cpp | 53 + .../cb_position_control_free_space.cpp | 195 + .../clients/components/cp_ue_pose.cpp | 59 + .../sm_dancebot_artgallery_ue.cpp | 27 + .../urdf/turtlebot3_waffle.urdf | 293 + .../worlds/ridgeback_race.world | 4864 ++++++ .../worlds/ridgeback_race_empty.world | 4193 +++++ .../worlds/ridgeback_race_no_lidar.world | 4881 ++++++ .../sm_dancebot_ue/docker/remove_container.sh | 1 + 110 files changed, 35538 insertions(+) create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/CHANGELOG.rst create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/CMakeLists.txt create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/README.md create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/.dockerignore create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/.gitignore create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/Dockerfile create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/README.md create mode 100755 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/attach_docker_container.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/build_docker.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/build_docker_humble.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/fastdds_setup.bash create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/frames_2023-08-05_01.06.53.gv create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/frames_2023-08-05_01.06.53.pdf create mode 100755 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/join_bash.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/join_editor.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/nvidia-driver-check.sh create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/openvpn/login.txt create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/openvpn/ovpnconfig.ovpn create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/openvpn/password.conf create mode 100755 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/openvpn_file_run.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/remove_container.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/run_docker_container_bash.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/run_docker_container_bash_development.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/run_docker_container_editor.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/run_editor_smacc.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/save_current_docker_image.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/start_container.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/stop_container.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/update_project_files.sh create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgalery_ue/states/st_back_on_road_waypoints_x.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgalery_ue/states/st_inital_road_waypoints_x.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/cl_nav2z/client_behaviors/cb_active_stop.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/cl_nav2z/client_behaviors/cb_load_waypoints_file.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/cl_nav2z/client_behaviors/cb_navigate_next_waypoint_free.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/cl_nav2z/client_behaviors/cb_position_control_free_space.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/cl_nav2z/client_behaviors/cb_pure_spinning.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/components/cp_ue_pose.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/modestates/ms_dance_bot_recovery_mode.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/modestates/ms_dance_bot_run_mode.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/orthogonals/or_navigation.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/orthogonals/or_obstacle_perception.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/sm_dancebot_artgallery_ue.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_forward_1.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_forward_2.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_loop_start.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_return_1.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_rotate_1.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_rotate_2.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/radial_motion_states/sti_radial_end_point.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/radial_motion_states/sti_radial_loop_start.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/radial_motion_states/sti_radial_return.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/radial_motion_states/sti_radial_rotate.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_forward_1.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_forward_2.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_forward_3.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_forward_4.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_loop_start.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_1.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_2.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_3.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_4.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_acquire_sensors.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_back_on_road_waypoints_x.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_final_state.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_inital_road_waypoints_x.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_navigate_field_waypoints_x.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_turn_around.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_f_pattern_1.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_radial_pattern_1.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_radial_pattern_2.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_radial_pattern_3.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_s_pattern_1.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/bringup_launch.py create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/husky_gazebo.launch.yaml create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/navigation_launch.py create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/online_sync_launch.py create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/rviz_launch.py create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/slam_launch.py create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/sm_dancebot_artgallery_ue_launch.py create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/package.xml create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/mapper_params_online_sync.yaml create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/nav2_params.yaml create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/navigation_tree.xml create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/waypoints_art_gallery.yaml create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client_husky/nav2_params.yaml create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client_husky/navigation_tree.xml create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client_husky/waypoints_plan.yaml create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client_turtlebot/nav2_params.yaml create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client_turtlebot/navigation_tree.xml create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client_turtlebot/waypoints_plan.yaml create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/sm_dancebot_artgallery_ue_config.yaml create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/rviz/nav2_default_view.rviz create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/rviz/nav2_namespaced_view.rviz create mode 100755 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/scripts/test_oscillation.py create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/scripts/transform_publisher.py create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/servers/led_action_server/action/LEDControl.action create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/servers/led_action_server/src/led_action_server_node.cpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/servers/service_node_3/service_node_3.py create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/servers/temperature_sensor_node/src/temperature_sensor_node.cpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/src/sm_dancebot_artgalery_ue/clients/client_behaviors/nav2z_client/cb_active_stop.cpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/src/sm_dancebot_artgallery_ue/clients/client_behaviors/nav2z_client/cb_active_stop.cpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/src/sm_dancebot_artgallery_ue/clients/client_behaviors/nav2z_client/cb_position_control_free_space.cpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/src/sm_dancebot_artgallery_ue/clients/components/cp_ue_pose.cpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/src/sm_dancebot_artgallery_ue/sm_dancebot_artgallery_ue.cpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/urdf/turtlebot3_waffle.urdf create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/worlds/ridgeback_race.world create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/worlds/ridgeback_race_empty.world create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/worlds/ridgeback_race_no_lidar.world diff --git a/smacc2_dev_tools/.vscode/launch.json b/smacc2_dev_tools/.vscode/launch.json index afc766017..7c4a9fa7a 100644 --- a/smacc2_dev_tools/.vscode/launch.json +++ b/smacc2_dev_tools/.vscode/launch.json @@ -398,6 +398,31 @@ } ] }, + { + "name": "(gdb) SmDanceBotArtGalleryUE", + "type": "cppdbg", + "request": "launch", + "program": "${workspaceFolder}/../install/sm_dancebot_artgallery_ue/lib/sm_dancebot_artgallery_ue/sm_dancebot_artgallery_ue_node", + "args": [ + "--ros-args", + "-r", + "__node:=SmDanceBotArtGalleryUE", + "--params-file", + "${workspaceFolder}/../install/sm_dancebot_artgallery_ue/share/sm_dancebot_artgallery_ue/params/sm_dancebot_artgallery_ue_config.yaml" + ], + "stopAtEntry": false, + "cwd": "${workspaceFolder}", + "environment": [], + "externalConsole": false, + "MIMode": "gdb", + "setupCommands": [ + { + "description": "Enable pretty-printing for gdb", + "text": "-enable-pretty-printing", + "ignoreFailures": true + } + ] + }, { "name": "(gdb) SmDanceBotWarehouse2", "type": "cppdbg", diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/CHANGELOG.rst b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/CHANGELOG.rst new file mode 100644 index 000000000..96ca799e4 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/CHANGELOG.rst @@ -0,0 +1,13067 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package sm_dancebot_artgallery_ue +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.3.6 (2023-03-12) +------------------ + +1.22.1 (2022-11-09) +------------------- +* pre-release +* Contributors: pabloinigoblasco + +* publisher +* Feature/warehouse 3 improvements (#313) + * improvements in navigation client behaviors and husky barrel demo + * many improvements in action client and cb sequence for hysky barrel search + * more and better navigation behaviors on husky barrel search demo + * functionality improvements in navigation and improvements of warehouse 3, format + * functionality improvements in navigation and improvements of warehouse 3 and husky + * format + * warehouse 3 improvements + * merge galactic + * merge fix + * minor +* improvements in navigation client behaviors and husky barrel demo (#311) + * improvements in navigation client behaviors and husky barrel demo + * many improvements in action client and cb sequence for hysky barrel search + * more and better navigation behaviors on husky barrel search demo + * functionality improvements in navigation and improvements of warehouse 3, format + * functionality improvements in navigation and improvements of warehouse 3 and husky + * format +* Feature/galactic rolling merge (#288) + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dancebot_artgallery_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dancebot_artgallery_ue introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dancebot_artgallery_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * more refinement in sm_dancebot_artgallery_ue + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dancebot_artgallery_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_artgallery_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_artgallery_ue tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nav2z renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dancebot_artgallery_ue + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * Update cb_navigate_global_position.hpp + * Merging code from backport foxy and updates about autoware (#208) + * minor changes + * replanning for all our examples + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dancebot_artgallery_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dancebot_artgallery_ue introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dancebot_artgallery_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * more refinement in sm_dancebot_artgallery_ue + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dancebot_artgallery_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_artgallery_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_artgallery_ue tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nav2z renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dancebot_artgallery_ue + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + * Add mergify rules file. + * Try fixing CI for rolling. (#209) + Merging to get backport working. + * Remove example things from Foxy CI setup. (#214) + * Add Autoware Auto Msgs into not-released dependencies. (#220) + * Fix rolling builds (#222) + * do not merge yet - Feature/odom tracker improvements and retry motion (#223) + * odom tracker improvements + * adding forward behavior retry funcionality + * removing warnings (#213) + * minor changes + * replanning for all our examples + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dancebot_artgallery_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dancebot_artgallery_ue introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dancebot_artgallery_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * more refinement in sm_dancebot_artgallery_ue + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dancebot_artgallery_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_artgallery_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_artgallery_ue tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nav2z renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dancebot_artgallery_ue + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + * some reordering fixes + * minor + * docker files for different revisions, warnings removval and more testing on navigation + * fixing docker for foxy and galactic + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + * dockerfiles (#225) + * Fix code generators (#221) + * Fix other build issues. + * Update SM template and make example code clearly visible. + * Remove use of node in the sm performance template. + * Updated templated to use Blackboard storage. + * Update template to resolve the global data correctly. + * Update sm_name.hpp + Co-authored-by: Pablo Iñigo Blasco + * Feature/retry behavior warehouse 1 (#226) + * minor changes + * replanning for all our examples + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dancebot_artgallery_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dancebot_artgallery_ue introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dancebot_artgallery_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * more refinement in sm_dancebot_artgallery_ue + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dancebot_artgallery_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_artgallery_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_artgallery_ue tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nav2z renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dancebot_artgallery_ue + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + * some reordering fixes + * minor + * docker files for different revisions, warnings removval and more testing on navigation + * fixing docker for foxy and galactic + * Update file for fake hardware simulation and add file for gazebo simulation. + * docker build files for all versions + * retry behavior warehouse 1 + * missing file + * minor format fix + * other minor changes + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + * fix broken source build (#227) + * Only rolling version should be pre-released on on master. (#230) + * Correct Focal-Rolling builds by fixing the version of rosdep yaml (#234) + * Update file for fake hardware simulation and add file for gazebo simulation. (#224) + * Update file for fake hardware simulation and add file for gazebo simulation. + * Add ignition file and update repos files. + * Feature/improvements warehouse3 (#228) + * minor changes + * replanning for all our examples + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dancebot_artgallery_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dancebot_artgallery_ue introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dancebot_artgallery_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * more refinement in sm_dancebot_artgallery_ue + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dancebot_artgallery_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_artgallery_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_artgallery_ue tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nav2z renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dancebot_artgallery_ue + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + * some reordering fixes + * minor + * docker files for different revisions, warnings removval and more testing on navigation + * fixing docker for foxy and galactic + * docker build files for all versions + * barrel demo + * barrel search build fix and warehouse3 + * fixing startup problems in warehouse 3 + * fixing format and minor + * minor + * progress in barrel husky + * minor + * barrel demo + * progress + * fixing broken build + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + * Feature/barrel - do not merge yet (#233) + * minor changes + * replanning for all our examples + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dancebot_artgallery_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dancebot_artgallery_ue introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dancebot_artgallery_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * more refinement in sm_dancebot_artgallery_ue + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dancebot_artgallery_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_artgallery_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_artgallery_ue tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nav2z renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dancebot_artgallery_ue + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + * some reordering fixes + * minor + * docker files for different revisions, warnings removval and more testing on navigation + * fixing docker for foxy and galactic + * docker build files for all versions + * barrel demo + * barrel search build fix and warehouse3 + * fixing startup problems in warehouse 3 + * fixing format and minor + * minor + * progress in barrel husky + * minor + * barrel demo + * minor + * barrel search updates + * making models local + * red picuup + * multiple controllable leds plugin + * progress in husky demo + * progressing in husky demo + * improving navigation behaviors + * more merge + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + * Feature/docker improvements march 2022 (#235) + * minor changes + * replanning for all our examples + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dancebot_artgallery_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dancebot_artgallery_ue introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dancebot_artgallery_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * more refinement in sm_dancebot_artgallery_ue + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dancebot_artgallery_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_artgallery_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_artgallery_ue tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nav2z renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dancebot_artgallery_ue + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + * some reordering fixes + * minor + * docker files for different revisions, warnings removval and more testing on navigation + * fixing docker for foxy and galactic + * docker build files for all versions + * barrel demo + * barrel search build fix and warehouse3 + * fixing startup problems in warehouse 3 + * fixing format and minor + * minor + * progress in barrel husky + * minor + * barrel demo + * minor + * barrel search updates + * making models local + * red picuup + * multiple controllable leds plugin + * progress in husky demo + * progressing in husky demo + * improving navigation behaviors + * more merge + * docker improvements + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + * Use correct upstream .repos files for source builds (#243) + * Correct mergify branch names (#246) + * Correct name of source-build job and bump version of action (#242) (#247) + Co-authored-by: Denis Štogl + Co-authored-by: Pablo Iñigo Blasco + * Update galactic source build job name (#250) + * Galactic source build: update .repos file, bump action version and use correct version of upstream packages (backport #241) (#248) + Co-authored-by: Denis Štogl + * fixing rolling build (#239) + * fixing rolling build + * trying to fix dependencies + * missing repo + * fixing to focal by the moment + * more fixing rolling build + * minor + * cache matrix rolling and source build package + * minor + * minor + * missing repo + * missing deps + * fixing building issue + * typo + * fixing broken build + * build fix + * restoring workflow files (#252) + * restoring files (#253) + * Fix checkout branches for scheduled builds (#254) + * correct checkout branch on scheduled build + * Update foxy-source-build.yml + * Feature/fixing husky build rolling (#257) + * restoring files + * making husky project build on rolling + * Feature/fixing husky build rolling (#258) + * restoring files + * making husky project build on rolling + * husky progress + * Update README.md (#262) + * Feature/fixing ur demos (#261) + * restoring files + * fixes + * Feature/fixing type string walker (#263) + * restoring files + * fixing type string walker threesome demo + * Update README.md (#266) + * Update README.md (#267) + * Update README.md (#268) + * Significant update in Getting Started Instructions (#269) + * Significant update in Getting Started Instructions + * Remove trailing spaces. + Co-authored-by: Denis Štogl + * fixing ur demo (#273) + * fix: initialise conditionFlag as false (#274) + * precommit fix (#280) + I merge this in red for focal-rolling because it was already broken anyway and it is only a minor update of the precommit + * Ignore packages which should not be released. + * Added changelogs. + * 0.4.0 + * Revert "Ignore packages which should not be released." + This reverts commit ee2cc86db3c0a24f9eb0a9e33217de3f7a691a1c. + * Fix urls to index.ros.org (#284) + * Fix foxy source build config to use repos file from foxy branch. (#285) + * adding spawn entity delays + * more on backport + * more on backport + * disappeared ur_msgs denis repo + * fixing sm_dancebot_artgallery_ue examples + * working on fix of image messages for husky_barrel demo + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + Co-authored-by: mergify[bot] <37929162+mergify[bot]@users.noreply.github.com> + Co-authored-by: brettpac +* Revert "Ignore packages which should not be released." + This reverts commit dec14a936a877b2ef722a6a32f1bf3df09312542. +* Contributors: Denis Štogl, Pablo Iñigo Blasco, pabloinigoblasco + +0.3.0 (2022-04-04) +------------------ + +0.0.0 (2022-11-09) +------------------ +* publisher +* Feature/warehouse 3 improvements (#313) + * improvements in navigation client behaviors and husky barrel demo + * many improvements in action client and cb sequence for hysky barrel search + * more and better navigation behaviors on husky barrel search demo + * functionality improvements in navigation and improvements of warehouse 3, format + * functionality improvements in navigation and improvements of warehouse 3 and husky + * format + * warehouse 3 improvements + * merge galactic + * merge fix + * minor +* improvements in navigation client behaviors and husky barrel demo (#311) + * improvements in navigation client behaviors and husky barrel demo + * many improvements in action client and cb sequence for hysky barrel search + * more and better navigation behaviors on husky barrel search demo + * functionality improvements in navigation and improvements of warehouse 3, format + * functionality improvements in navigation and improvements of warehouse 3 and husky + * format +* Feature/galactic rolling merge (#288) + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dancebot_artgallery_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dancebot_artgallery_ue introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dancebot_artgallery_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * more refinement in sm_dancebot_artgallery_ue + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dancebot_artgallery_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_artgallery_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_artgallery_ue tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nav2z renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dancebot_artgallery_ue + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * Update cb_navigate_global_position.hpp + * Merging code from backport foxy and updates about autoware (#208) + * minor changes + * replanning for all our examples + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dancebot_artgallery_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dancebot_artgallery_ue introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dancebot_artgallery_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * more refinement in sm_dancebot_artgallery_ue + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dancebot_artgallery_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_artgallery_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_artgallery_ue tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nav2z renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dancebot_artgallery_ue + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + * Add mergify rules file. + * Try fixing CI for rolling. (#209) + Merging to get backport working. + * Remove example things from Foxy CI setup. (#214) + * Add Autoware Auto Msgs into not-released dependencies. (#220) + * Fix rolling builds (#222) + * do not merge yet - Feature/odom tracker improvements and retry motion (#223) + * odom tracker improvements + * adding forward behavior retry funcionality + * removing warnings (#213) + * minor changes + * replanning for all our examples + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dancebot_artgallery_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dancebot_artgallery_ue introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dancebot_artgallery_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * more refinement in sm_dancebot_artgallery_ue + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dancebot_artgallery_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_artgallery_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_artgallery_ue tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nav2z renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dancebot_artgallery_ue + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + * some reordering fixes + * minor + * docker files for different revisions, warnings removval and more testing on navigation + * fixing docker for foxy and galactic + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + * dockerfiles (#225) + * Fix code generators (#221) + * Fix other build issues. + * Update SM template and make example code clearly visible. + * Remove use of node in the sm performance template. + * Updated templated to use Blackboard storage. + * Update template to resolve the global data correctly. + * Update sm_name.hpp + Co-authored-by: Pablo Iñigo Blasco + * Feature/retry behavior warehouse 1 (#226) + * minor changes + * replanning for all our examples + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dancebot_artgallery_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dancebot_artgallery_ue introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dancebot_artgallery_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * more refinement in sm_dancebot_artgallery_ue + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dancebot_artgallery_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_artgallery_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_artgallery_ue tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nav2z renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dancebot_artgallery_ue + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + * some reordering fixes + * minor + * docker files for different revisions, warnings removval and more testing on navigation + * fixing docker for foxy and galactic + * Update file for fake hardware simulation and add file for gazebo simulation. + * docker build files for all versions + * retry behavior warehouse 1 + * missing file + * minor format fix + * other minor changes + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + * fix broken source build (#227) + * Only rolling version should be pre-released on on master. (#230) + * Correct Focal-Rolling builds by fixing the version of rosdep yaml (#234) + * Update file for fake hardware simulation and add file for gazebo simulation. (#224) + * Update file for fake hardware simulation and add file for gazebo simulation. + * Add ignition file and update repos files. + * Feature/improvements warehouse3 (#228) + * minor changes + * replanning for all our examples + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dancebot_artgallery_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dancebot_artgallery_ue introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dancebot_artgallery_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * more refinement in sm_dancebot_artgallery_ue + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dancebot_artgallery_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_artgallery_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_artgallery_ue tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nav2z renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dancebot_artgallery_ue + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + * some reordering fixes + * minor + * docker files for different revisions, warnings removval and more testing on navigation + * fixing docker for foxy and galactic + * docker build files for all versions + * barrel demo + * barrel search build fix and warehouse3 + * fixing startup problems in warehouse 3 + * fixing format and minor + * minor + * progress in barrel husky + * minor + * barrel demo + * progress + * fixing broken build + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + * Feature/barrel - do not merge yet (#233) + * minor changes + * replanning for all our examples + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dancebot_artgallery_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dancebot_artgallery_ue introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dancebot_artgallery_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * more refinement in sm_dancebot_artgallery_ue + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dancebot_artgallery_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_artgallery_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_artgallery_ue tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nav2z renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dancebot_artgallery_ue + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + * some reordering fixes + * minor + * docker files for different revisions, warnings removval and more testing on navigation + * fixing docker for foxy and galactic + * docker build files for all versions + * barrel demo + * barrel search build fix and warehouse3 + * fixing startup problems in warehouse 3 + * fixing format and minor + * minor + * progress in barrel husky + * minor + * barrel demo + * minor + * barrel search updates + * making models local + * red picuup + * multiple controllable leds plugin + * progress in husky demo + * progressing in husky demo + * improving navigation behaviors + * more merge + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + * Feature/docker improvements march 2022 (#235) + * minor changes + * replanning for all our examples + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dancebot_artgallery_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dancebot_artgallery_ue introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dancebot_artgallery_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * more refinement in sm_dancebot_artgallery_ue + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dancebot_artgallery_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_artgallery_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_artgallery_ue tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nav2z renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dancebot_artgallery_ue + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + * some reordering fixes + * minor + * docker files for different revisions, warnings removval and more testing on navigation + * fixing docker for foxy and galactic + * docker build files for all versions + * barrel demo + * barrel search build fix and warehouse3 + * fixing startup problems in warehouse 3 + * fixing format and minor + * minor + * progress in barrel husky + * minor + * barrel demo + * minor + * barrel search updates + * making models local + * red picuup + * multiple controllable leds plugin + * progress in husky demo + * progressing in husky demo + * improving navigation behaviors + * more merge + * docker improvements + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + * Use correct upstream .repos files for source builds (#243) + * Correct mergify branch names (#246) + * Correct name of source-build job and bump version of action (#242) (#247) + Co-authored-by: Denis Štogl + Co-authored-by: Pablo Iñigo Blasco + * Update galactic source build job name (#250) + * Galactic source build: update .repos file, bump action version and use correct version of upstream packages (backport #241) (#248) + Co-authored-by: Denis Štogl + * fixing rolling build (#239) + * fixing rolling build + * trying to fix dependencies + * missing repo + * fixing to focal by the moment + * more fixing rolling build + * minor + * cache matrix rolling and source build package + * minor + * minor + * missing repo + * missing deps + * fixing building issue + * typo + * fixing broken build + * build fix + * restoring workflow files (#252) + * restoring files (#253) + * Fix checkout branches for scheduled builds (#254) + * correct checkout branch on scheduled build + * Update foxy-source-build.yml + * Feature/fixing husky build rolling (#257) + * restoring files + * making husky project build on rolling + * Feature/fixing husky build rolling (#258) + * restoring files + * making husky project build on rolling + * husky progress + * Update README.md (#262) + * Feature/fixing ur demos (#261) + * restoring files + * fixes + * Feature/fixing type string walker (#263) + * restoring files + * fixing type string walker threesome demo + * Update README.md (#266) + * Update README.md (#267) + * Update README.md (#268) + * Significant update in Getting Started Instructions (#269) + * Significant update in Getting Started Instructions + * Remove trailing spaces. + Co-authored-by: Denis Štogl + * fixing ur demo (#273) + * fix: initialise conditionFlag as false (#274) + * precommit fix (#280) + I merge this in red for focal-rolling because it was already broken anyway and it is only a minor update of the precommit + * Ignore packages which should not be released. + * Added changelogs. + * 0.4.0 + * Revert "Ignore packages which should not be released." + This reverts commit ee2cc86db3c0a24f9eb0a9e33217de3f7a691a1c. + * Fix urls to index.ros.org (#284) + * Fix foxy source build config to use repos file from foxy branch. (#285) + * adding spawn entity delays + * more on backport + * more on backport + * disappeared ur_msgs denis repo + * fixing sm_dancebot_artgallery_ue examples + * working on fix of image messages for husky_barrel demo + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + Co-authored-by: mergify[bot] <37929162+mergify[bot]@users.noreply.github.com> + Co-authored-by: brettpac +* Revert "Ignore packages which should not be released." + This reverts commit dec14a936a877b2ef722a6a32f1bf3df09312542. +* Ignore packages which should not be released. +* Feature/master rolling to galactic backport (#236) + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dancebot_artgallery_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dancebot_artgallery_ue introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dancebot_artgallery_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * more refinement in sm_dancebot_artgallery_ue + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dancebot_artgallery_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_artgallery_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_artgallery_ue tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nav2z renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dancebot_artgallery_ue + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * replanning for all our examples + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dancebot_artgallery_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dancebot_artgallery_ue introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dancebot_artgallery_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * more refinement in sm_dancebot_artgallery_ue + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dancebot_artgallery_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_artgallery_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_artgallery_ue tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nav2z renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dancebot_artgallery_ue + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * Update cb_navigate_global_position.hpp + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + * Merging code from backport foxy and updates about autoware (#208) + * minor changes + * replanning for all our examples + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dancebot_artgallery_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dancebot_artgallery_ue introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dancebot_artgallery_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * more refinement in sm_dancebot_artgallery_ue + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dancebot_artgallery_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_artgallery_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_artgallery_ue tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nav2z renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dancebot_artgallery_ue + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + * Add mergify rules file. + * Try fixing CI for rolling. (#209) + Merging to get backport working. + * some reordering fixes + * Remove example things from Foxy CI setup. (#214) + * Add Autoware Auto Msgs into not-released dependencies. (#220) + * Fix rolling builds (#222) + * do not merge yet - Feature/odom tracker improvements and retry motion (#223) + * odom tracker improvements + * adding forward behavior retry funcionality + * minor + * docker files for different revisions, warnings removval and more testing on navigation + * fixing docker for foxy and galactic + * removing warnings (#213) + * minor changes + * replanning for all our examples + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dancebot_artgallery_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dancebot_artgallery_ue introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dancebot_artgallery_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * more refinement in sm_dancebot_artgallery_ue + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dancebot_artgallery_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_artgallery_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_artgallery_ue tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nav2z renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dancebot_artgallery_ue + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + * some reordering fixes + * minor + * docker files for different revisions, warnings removval and more testing on navigation + * fixing docker for foxy and galactic + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + * docker build files for all versions + * dockerfiles (#225) + * Fix code generators (#221) + * Fix other build issues. + * Update SM template and make example code clearly visible. + * Remove use of node in the sm performance template. + * Updated templated to use Blackboard storage. + * Update template to resolve the global data correctly. + * Update sm_name.hpp + Co-authored-by: Pablo Iñigo Blasco + * Feature/retry behavior warehouse 1 (#226) + * minor changes + * replanning for all our examples + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dancebot_artgallery_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dancebot_artgallery_ue introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dancebot_artgallery_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * more refinement in sm_dancebot_artgallery_ue + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dancebot_artgallery_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_artgallery_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_artgallery_ue tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nav2z renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dancebot_artgallery_ue + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + * some reordering fixes + * minor + * docker files for different revisions, warnings removval and more testing on navigation + * fixing docker for foxy and galactic + * Update file for fake hardware simulation and add file for gazebo simulation. + * docker build files for all versions + * retry behavior warehouse 1 + * missing file + * minor format fix + * other minor changes + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + * barrel demo + * barrel search build fix and warehouse3 + * fixing startup problems in warehouse 3 + * fix broken source build (#227) + * fixing format and minor + * minor + * progress in barrel husky + * minor + * Only rolling version should be pre-released on on master. (#230) + * barrel demo + * minor + * barrel search updates + * making models local + * red picuup + * Correct Focal-Rolling builds by fixing the version of rosdep yaml (#234) + * multiple controllable leds plugin + * progress in husky demo + * Update file for fake hardware simulation and add file for gazebo simulation. (#224) + * Update file for fake hardware simulation and add file for gazebo simulation. + * Add ignition file and update repos files. + * progressing in husky demo + * improving navigation behaviors + * Feature/improvements warehouse3 (#228) + * minor changes + * replanning for all our examples + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dancebot_artgallery_ue + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dancebot_artgallery_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dancebot_artgallery_ue visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dancebot_artgallery_ue introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dancebot_artgallery_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * more refinement in sm_dancebot_artgallery_ue + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dancebot_artgallery_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_artgallery_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_artgallery_ue tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nav2z renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dance_bot + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + * some reordering fixes + * minor + * docker files for different revisions, warnings removval and more testing on navigation + * fixing docker for foxy and galactic + * docker build files for all versions + * barrel demo + * barrel search build fix and warehouse3 + * fixing startup problems in warehouse 3 + * fixing format and minor + * minor + * progress in barrel husky + * minor + * barrel demo + * progress + * fixing broken build + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + * more merge + * docker improvements + * master rolling to galactic backport + * fixing build + * testing dance bot demos + * updating galactic repos + * runtime dependency + * restoring ur dependency + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco +* Backport/initial to galactic (#61) + * reformatting the whole project + * Remove test phase from CMake and remove dependencies from package.xml. + * Compile with navigation and slam_toolbox. + * Enable all packages to compile. + * Resolve missing dependency in smacc_msgs and reorganize them for better overview. + * getLogger functionality and refactoring + * broken sm_respira + * sm_respira code + * Update README.md + ## Additions + - build-status table + - detailed install instructions (adjusted from [here](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver#readme)) + ## Changes + - default build type as `Release` (it is faster than `Debug` and executables are smaller) + - updated examples section + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Update tracing/ManualTracing.md + * reactivating smacc2 nav clients for rolling via submodules + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * edited tracing.md to reflect new tracing event names + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * Correct build-overview table. + * Update and unify CI configurations. + * Use tf_geometry_msgs.h in galactic. + * Use galactic branches in .repos-file. + Co-authored-by: pabloinigoblasco + Co-authored-by: reelrbtx + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: brettpac +* Contributors: Denis Štogl, Pablo Iñigo Blasco, pabloinigoblasco diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/CMakeLists.txt b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/CMakeLists.txt new file mode 100644 index 000000000..7c8da8a7b --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/CMakeLists.txt @@ -0,0 +1,113 @@ +cmake_minimum_required(VERSION 3.5) +project(sm_dancebot_artgallery_ue) + +# 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(ament_cmake_python REQUIRED) +find_package(Boost COMPONENTS thread REQUIRED) + +find_package(smacc2 REQUIRED) +find_package(std_msgs REQUIRED) +find_package(nav2z_client REQUIRED) + +find_package(ros_publisher_client REQUIRED) +find_package(multirole_sensor_client REQUIRED) + +find_package(sr_all_events_go REQUIRED) +find_package(sr_event_countdown REQUIRED) +find_package(sr_conditional REQUIRED) +find_package(ros_timer_client REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(ue_msgs REQUIRED) + +set(dependencies + smacc2 + std_msgs + sensor_msgs + nav2z_client + ros_publisher_client + multirole_sensor_client + sr_all_events_go + sr_event_countdown + sr_conditional + ros_timer_client + visualization_msgs + ue_msgs +) + +rosidl_generate_interfaces(${PROJECT_NAME} + "servers/led_action_server/action/LEDControl.action" + DEPENDENCIES builtin_interfaces std_msgs action_msgs sensor_msgs +) + +include_directories(include + ${Boost_INCLUDE_DIRS} + ${CMAKE_BINARY_DIR}/rosidl_generator_cpp) + +add_executable(${PROJECT_NAME}_node + src/${PROJECT_NAME}/sm_dancebot_artgallery_ue.cpp + src/${PROJECT_NAME}/clients/client_behaviors/nav2z_client/cb_position_control_free_space.cpp + src/${PROJECT_NAME}/clients/components/cp_ue_pose.cpp + src/${PROJECT_NAME}/clients/client_behaviors/nav2z_client/cb_active_stop.cpp + ) + +add_executable(temperature_sensor_node_${PROJECT_NAME} servers/temperature_sensor_node/src/temperature_sensor_node.cpp) +add_executable(led_action_server_node_${PROJECT_NAME} servers/led_action_server/src/led_action_server_node.cpp) + +ament_target_dependencies(${PROJECT_NAME}_node ${dependencies}) +target_link_libraries(${PROJECT_NAME}_node ${Boost_LIBS} ${PROJECT_NAME}__rosidl_typesupport_cpp) + +ament_target_dependencies(led_action_server_node_${PROJECT_NAME} ${dependencies}) +set_target_properties(led_action_server_node_${PROJECT_NAME} PROPERTIES OUTPUT_NAME "led_action_server_node") +target_link_libraries(led_action_server_node_${PROJECT_NAME} ${Boost_LIBS} ${PROJECT_NAME}__rosidl_typesupport_cpp) + +ament_target_dependencies(temperature_sensor_node_${PROJECT_NAME} ${dependencies}) +set_target_properties(temperature_sensor_node_${PROJECT_NAME} PROPERTIES OUTPUT_NAME "temperature_sensor_node") + +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) +ament_export_dependencies(${dependencies}) + + +if(BUILD_TESTING) +endif() + +install(TARGETS + ${PROJECT_NAME}_node + temperature_sensor_node_${PROJECT_NAME} + led_action_server_node_${PROJECT_NAME} + + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +#ament_python_install_package(${PROJECT_NAME} +# PACKAGE_DIR servers/service_node_3 +# ) + + +install(FILES + servers/service_node_3/service_node_3.py + scripts/transform_publisher.py + DESTINATION + lib/${PROJECT_NAME} + PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READ + GROUP_EXECUTE GROUP_READ) + +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY params DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY urdf DESTINATION share/${PROJECT_NAME}) + +ament_package() diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/README.md b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/README.md new file mode 100644 index 000000000..8fa9efb95 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/README.md @@ -0,0 +1,55 @@ +

State Machine Diagram

+ + ![sm_dance_bot](docs/SmDanceBot_2023-5-26_151817.svg) + +

Description

A full-featured state machine example, that highlights the capabilities of SMACC2 & the ROS2 Navigation Stack via the MoveBaseZ Client. +.

+Doxygen Namespace & Class Reference + +

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 ~/colcon_ws/install/setup.bash +``` + +And then run the launch file... + +``` +ros2 launch sm_dance_bot sm_dance_bot_launch.py +``` +

Headless launch

+ +Alternatively, you can also launch the gazebo simulator in headless mode: +``` +ros2 launch sm_dance_bot sm_dance_bot_launch.py headless:=True +``` +

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_dancebot_artgallery_ue/docker/.dockerignore b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/.dockerignore new file mode 100644 index 000000000..e69de29bb diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/.gitignore b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/.gitignore new file mode 100644 index 000000000..167d48f57 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/.gitignore @@ -0,0 +1 @@ +UE5.1 diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/Dockerfile b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/Dockerfile new file mode 100644 index 000000000..d082635a3 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/Dockerfile @@ -0,0 +1,131 @@ +ARG ROS_DISTRO +ARG GIT_BRANCH +ARG UBUNTU_VERSION + +FROM ros:$ROS_DISTRO-ros-base-$UBUNTU_VERSION + +ARG ROS_DISTRO +ARG GIT_BRANCH +ARG LOCAL_FOLDER_SOURCE=1 +ENV DEBIAN_FRONTEND=noninteractive + +RUN apt-get update \ + && apt-get upgrade -y \ + && apt-get clean + +RUN apt-get update && apt-get install -q -y --no-install-recommends \ + dirmngr \ + gnupg2 \ + lsb-release \ + python3-colcon-ros \ + && apt-get clean \ + && apt upgrade -y --with-new-pkgs + +RUN apt-get install -y nvidia-driver-525 + +RUN apt install -y xdg-user-dirs + +ENV USER=ros2_ws +RUN bash -c "useradd -ms /bin/bash $USER" +RUN usermod -aG sudo $USER +RUN echo '%sudo ALL=(ALL) NOPASSWD:ALL' >> /etc/sudoers +RUN chown -R $USER:$USER /home/$USER + +USER $USER +WORKDIR /home/$USER + +WORKDIR "/home/$USER/src" + +RUN echo "copying current code version to docker image:" +#ADD . /home/$USER/src/SMACC2 +#WORKDIR "/home/$USER" + +# install dependencies and build +# RUN vcs import src --skip-existing --input src/SMACC2/.github/SMACC2-not-released.$ROS_DISTRO.repos +# RUN ls src + +# RUN apt update +# RUN rosdep install --from-paths src --ignore-src -r -y +# RUN apt-get update && apt-get install -q -y --no-install-recommends xterm + +# RUN bash -c "source /opt/ros/$ROS_DISTRO/setup.sh && colcon build --merge-install --parallel-workers 1" + +COPY --chown=$USER:$USER smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/UE5.1/UnrealEngine /home/$USER/src/UE5.1/UnrealEngine + +ENV UE5_DIR=/home/$USER/src/UE5.1/UnrealEngine +ENV PROJECT_BRANCH=smacc2_devel_stable +ENV PROJECT_NAME=ue_project_1 + +RUN sudo apt-get install -y wget gpg +RUN wget -qO- https://packages.microsoft.com/keys/microsoft.asc | sudo gpg --dearmor > packages.microsoft.gpg +RUN sudo install -D -o root -g root -m 644 packages.microsoft.gpg /etc/apt/keyrings/packages.microsoft.gpg +RUN sudo sh -c 'echo "deb [arch=amd64,arm64,armhf signed-by=/etc/apt/keyrings/packages.microsoft.gpg] https://packages.microsoft.com/repos/code stable main" > /etc/apt/sources.list.d/vscode.list' -f packages.microsoft.gpg + +RUN sudo apt install -y apt-transport-https +RUN sudo apt update +RUN sudo apt install -y code git-lfs nano # or code-insiders + +# smacc2_devel +RUN git clone --recurse-submodules https://github.com/robosoft-ai/${PROJECT_NAME}.git -b $PROJECT_BRANCH + +WORKDIR "/home/$USER/src/${PROJECT_NAME}" +# RUN git checkout abdf483ce14f477d8f255fe19f3527975b3a9dbb +# RUN cd Plugins/rclUE && git checkout bae993fa + +#RUN rm -rf Plugins/RapyutaSimulationPlugins && git clone https://github.com/robosoft-ai/UE-Plugins.git Plugins/RapyutaSimulationPlugins +#RUN cd Plugins/RapyutaSimulationPlugins && git checkout bfac015 + +RUN git-lfs pull && git submodule foreach git-lfs pull +RUN ls && echo ${UE5_DIR} +RUN ./update_project_files.sh +ENV UE5_DIR=/home/$USER/src/UE5.1 + +# installing Dependencies +RUN wget http://security.ubuntu.com/ubuntu/pool/universe/t/tinyxml2/libtinyxml2-6a_7.0.0+dfsg-1build1_amd64.deb +RUN wget http://archive.ubuntu.com/ubuntu/pool/main/o/openssl/libssl1.1_1.1.0g-2ubuntu4_amd64.deb +RUN sudo dpkg -i libtinyxml2-6a_7.0.0+dfsg-1build1_amd64.deb +RUN sudo dpkg -i libssl1.1_1.1.0g-2ubuntu4_amd64.deb + +ENV LD_LIBRARY_PATH=/usr/local/cuda/lib64:/usr/lib/x86_64-linux-gnu/gazebo-11/plugins:/opt/ros/humble/opt/rviz_ogre_vendor/lib:/opt/ros/humble/lib/x86_64-linux-gnu:/home/$USER/src/${PROJECT_NAME}/Plugins/rclUE/ThirdParty/ros/lib/ +ENV UE5_DIR=/home/$USER/src/UE5.1/UnrealEngine + +COPY --chown=$USER:$USER smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/nvidia-driver-check.sh /home/$USER/src/${PROJECT_NAME}/nvidia-driver-check.sh +RUN cat Makefile +RUN ./update_project_files.sh +#RUN bash + +RUN make ${PROJECT_NAME}Editor + +# # COPY smacc2 /home/$USER/src/SMACC2/smacc2 +# RUN mkdir -p /home/$USER/src/SMACC2/ +# WORKDIR "/home/$USER/src/SMACC2" +# COPY --chown=$USER:$USER smacc2_client_library /home/$USER/src/SMACC2/smacc2_client_library +# COPY --chown=$USER:$USER smacc2_event_generator_library /home/$USER/src/SMACC2/smacc2_event_generator_library +# COPY --chown=$USER:$USER smacc2_performance_tools /home/$USER/src/SMACC2/smacc2_performance_tools +# COPY --chown=$USER:$USER smacc2_state_reactor_library /home/$USER/src/SMACC2/smacc2_state_reactor_library +# COPY --chown=$USER:$USER smacc2 /home/$USER/src/SMACC2/smacc2 +# COPY --chown=$USER:$USER smacc2_dev_tools /home/$USER/src/SMACC2/smacc2_dev_tools +# COPY --chown=$USER:$USER smacc2_msgs /home/$USER/src/SMACC2/smacc2_msgs +# #smacc2_sm_reference_library + +# #RUN git clone +# RUN sudo apt-get update +# RUN rosdep update +# RUN rosdep install --ignore-src --from-paths . -y -r +# RUN sudo apt-get update && sudo apt-get install -y ros-humble-nav2-bringup ros-humble-rviz2 + +# RUN sudo apt-get install -y openvpn +# #COPY --chown=$USER:$USER smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/run_editor_smacc.sh /home/$USER/src/${PROJECT_NAME}/run_editor_smacc.sh + + +# RUN touch /home/$USER/src/${PROJECT_NAME}/COLCON_IGNORE +# RUN touch /home/$USER/src/UE5.1/COLCON_IGNORE +# WORKDIR "/home/$USER/" +# RUN bash -c "source /opt/ros/humble/setup.bash && colcon build --symlink-install" + +# # BASIC VPN FRAMEWORK --- +RUN mkdir /home/$USER/ovpnconfig + +WORKDIR "/home/$USER/src/${PROJECT_NAME}" + +# #ENV LD_LIBRARY_PATH=/home/ros2_ws/src/ue_project_1/Plugins/rclUE/ThirdParty/ros/lib/:/usr/local/cuda/lib64:/usr/lib/x86_64-linux-gnu/gazebo-11/plugins:/opt/ros/humble/opt/rviz_ogre_vendor/lib:/opt/ros/humble/lib/x86_64-linux-gnu: diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/README.md b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/README.md new file mode 100644 index 000000000..679de5103 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/README.md @@ -0,0 +1,296 @@ +# SMACC2 and UNREAL ENGINE EDITOR +The README.md file provides a comprehensive guide for setting up and utilizing the Unreal Engine Docker environment. The document covers the necessary prerequisites, such as installing Docker and NVIDIA-Docker2, and provides step-by-step instructions for downloading and loading the prebuilt Docker image containing Unreal Engine. Additionally, it explains how to run the Unreal Editor within the container, create new containers for debugging purposes, and connect the container to a VPN if required. The README.md also includes important notes and optional instructions for rebuilding and running SMACC state machines inside the container. Whether you are new to Docker or an experienced user, this document will help you navigate the process of working with Unreal Engine in a Docker environment effectively. + +## Important Notes about the Solution + +Here are some important notes regarding the solution: + +**Repos Versioning:** Not all versions of `rclUE`, `turtlebot3Editor`, and `RapyputaPlugins` are compatible with each other. Ensuring consistency among them can be challenging, especially when considering that the container maps/volumes some volumes with external contents. The container is already designed to have a correct combination of all of them, but this is hardcoded in the Dockerfile and can be improved. + +**DDS Configuration:** There may be communication issues between the Docker container nodes and host nodes. As a workaround, we currently use `cyclonedds` on the host (until a uniform solution using `fastrtps` is found). To set this up, add the following line to the `.bashrc` file on the host: +``` +export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp +``` + +***Automatic container nvidia driver update*** +The host and the container must have the same nvidia-driver in order to run the ue editor and simulation. +There is a mechanism implemented to automatically sync the driver. The current driver version is passed from the host to the container and then it is updated in the container if that is required. That is done in the nvidia-check.sh script. + +## Pre-Requisites +Before proceeding with the instructions, ensure that you have the necessary prerequisites installed on your system. + +### Docker Installation +To install Docker, follow these steps: + +1. Update the package list: + ``` + sudo apt-get update + ``` + +2. Install the required dependencies: + ``` + sudo apt-get install apt-transport-https ca-certificates curl gnupg-agent software-properties-common + ``` + +3. Add Docker's official GPG key: + ``` + curl -fsSL https://download.docker.com/linux/ubuntu/gpg | sudo apt-key add - + ``` + +4. Add the Docker repository: + ``` + sudo add-apt-repository "deb [arch=amd64] https://download.docker.com/linux/ubuntu bionic stable" + ``` + +5. Update the package list again: + ``` + sudo apt-get update + ``` + +6. Install Docker: + ``` + sudo apt-get install docker-ce + ``` + +### NVIDIA-Docker2 Installation +Ensure that you have NVIDIA-Docker2 installed by executing the following commands: + +1. Add the NVIDIA-Docker GPG key: + ``` + curl -s -L https://nvidia.github.io/nvidia-docker/gpgkey | sudo apt-key add - + ``` + +2. Determine the distribution: + ``` + distribution=$(. /etc/os-release; echo $ID$VERSION_ID) + ``` + +3. Add the NVIDIA-Docker repository: + ``` + curl -s -L https://nvidia.github.io/nvidia-docker/$distribution/nvidia-docker.list | sudo tee /etc/apt/sources.list.d/nvidia-docker.list + ``` + +4. Update the package list: + ``` + sudo apt-get update + ``` + +5. Install NVIDIA-Docker2: + ``` + sudo apt-get install nvidia-docker2 + ``` + +6. Restart the Docker service: + ``` + sudo systemctl restart docker.service + ``` + +### Adding User to Docker Group +To allow a user to run Docker, add the user to the Docker group. Replace `` with the actual user ID: + +``` +sudo usermod -a -G docker +``` + +### Testing the Installation +To test your Docker and NVIDIA-Docker2 installations, run the following command: + +``` +sudo nvidia-docker run --rm nvidia/cuda:10.2-base-ubuntu18.04 nvidia-smi +``` + +## Unreal Engine Docker Image +This section provides instructions for downloading, loading, and running the prebuilt Docker image containing Unreal Engine. + +### Downloading the Prebuilt Image +Download the prebuilt Docker image for Unreal Engine from the provided source. + +### Loading the Docker Image +To load the downloaded image into your Docker image database, use the following command: + +``` +sudo docker load -i ue_editor_rclue.tar +``` + +### (Alternative) Building the Docker Image Locally +If you want to modify the Docker image using a Dockerfile and rebuild it, follow these steps: + +1. Create a link to the UE5.1 folder inside the `sm_dancebot_artgallery_ue/docker` folder: + ``` + mount --bind UE5.1/UnrealEngine + ``` + * Recall the must contain the following folders: `Engine`, `FeaturePacks` and `Template` + +2. Build the image locally by running the following command: + ``` + ./build_docker_humble.sh + ``` + +## Using the Docker Image +This section explains how to run and create a new container from the `ue_editor_rcl` Docker image. + +### Running/Creating a New Container from the ue_editor_rcl Docker Image + +To run the Unreal Editor inside the container, you need to use some auxiliary scripts located in the `sm_dancebot_artgallery_ue` example. Follow these steps: + +1. Download the current SMACC2 repository. + +2. Navigate to the `sm_dancebot_artgallery_ue/docker` folder. + +3. Execute the following command: + ``` + ./run_docker_container_editor.sh + ``` + + This will run and create a new container using the `ue_editor_rcl` Docker image. + +4. The Unreal Engine editor will automatically open in "edition mode." You can launch the simulation with Turtlebot topics accessible from both the container and the host computer by clicking the "play" button. + + Note: When you close the editor, the container will also be finished, but it will remain installed. You can reopen the editor using the command: + ``` + ./start_container.sh + ``` + +### (Alternative) Running/Creating a New Container for Container Debugging + +There is an alternative way to create the container in daemon mode, where the lifetime of the container is not tied to the Unreal Editor window. This is useful, especially for developing new features for the container. To create the container in this mode,: + +1. Execute the following command: + ``` + ./run_docker_container_bash.sh + ``` + + This will create and start a new container as a daemon. The container will be available even after restarting. It is capable of opening the Unreal Engine editor with ROS2, but the editor will not open automatically. + +2. Execute the editor proccess: + ``` + ./join_editor.sh + ``` + +The Unreal Engine editor will automatically open in "edition mode." You can launch the simulation with Turtlebot topics accessible from both the container and the host computer by clicking the "play" button. + +### Stopping and Removing a Running Container + +If you need to reset everything and remove the existing container, follow these steps: + +1. Execute the following command: + ``` + ./stop_container.sh + ``` + + This will stop the running container. + +2. Execute the following command: + ``` + ./remove_container.sh + ``` + + This will remove the existing container. + +### Joining the Container via Bash + +To enter the Docker container and debug or test things from the command line, use the following command: + +``` +./join_bash.sh +``` + +### Connecting the Container to VPN + +The prototype for connecting the container to a VPN has already been completed. For detailed instructions, please refer to the relevant documentation. + +### Optionally Rebuilding and Running a SMACC State Machine inside the Container + +Note that the `ue_editor_rcl` containers are run by mapping the current `smacc2` source folder, allowing for mixed development between the host (using VSCode) and the container (for compiling and running the state machines). + +The SMACC2 source code is already prebuilt inside the image, making it available to any container. However, if you need to modify the `smacc2` code or examples and rebuild it, follow these steps: + +1. Change to the home directory: + ``` + cd ~/ + ``` + +2. Source the Humble ROS 2 installation: + ``` + source /opt/ros/humble/setup.bash + ``` + +3. Build the `smacc2` code: + ``` + colcon build + ``` + +4. To run your current demo, execute the following commands after building: + ``` + source install/setup.bash + ros2 launch sm_dancebot_artgallery_ue sm_dancebot_artgallery_ue_launch.py + ``` + +Enjoy experimenting with Unreal Engine in your Docker environment! + + + +# Brett's runtime notes + +You'll need to open three terminals for this demo. + One for the container where you'll run Unreal Engine + One to run the state machine (on the host) + One for the RTA (on the host) + +### Terminal 1 - For UE5 Simulation in the container + +1. Download the current SMACC2 repository. + ``` + cd ~/workspace/humble_ws/src + git clone https://github.com/robosoft-ai/SMACC2.git + + ``` +2. Build the workspace + + ``` + cd ~/workspace/humble_ws/ + source /opt/ros/humble/setup.bash + colcon build + + ``` + Once everything is done building... +3. Navigate to the `sm_dancebot_artgallery_ue/docker` folder. + + ``` + cd ~/workspace/humble_ws/src/SMACC2/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker + ``` +4. Execute the following command: + ``` + ./run_docker_container_editor.sh + ``` + This will create and start a new container as a daemon. The container will be available even after restarting. It is capable of opening the Unreal Engine editor with ROS2, but the editor will not open automatically. + The Unreal Engine editor will automatically open in "edition mode." You can launch the simulation with Turtlebot topics accessible from both the container and the host computer by clicking the "play" button. + +### Terminal 2 - for the state machine on the host + +1. Add the following line to the `.bashrc` file on the host: + ``` + export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp + ``` +2. Source the workspace you just built + ``` + source ~/workspace/humble_ws/install/setup.bash + ``` +3. Launch the state machine + ``` + ros2 launch sm_dancebot_artgallery_ue sm_dancebot_artgallery_ue_launch.py + ``` + This will launch the state machine application, rviz and other required nodes. + +### Terminal 3 - for the SMACC2_RTA on the host +1. Source the install + ``` + source /opt/ros/humble/setup.bash + ``` +2. Launch the SMACC2_RTA + ``` + ros2 run smacc2_rta smacc2_rta + ``` +3. Once the RTA is launched, in the upper left corner select State Machine/Available State Machines/SmDanceBotUE + +And you should be all set. diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/attach_docker_container.sh b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/attach_docker_container.sh new file mode 100755 index 000000000..37343f136 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/attach_docker_container.sh @@ -0,0 +1 @@ +sudo docker attach smacc_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/build_docker.sh b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/build_docker.sh new file mode 100755 index 000000000..b0d1f82a3 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/build_docker.sh @@ -0,0 +1,21 @@ +#!/bin/sh +DIR="$(dirname "$(realpath "$0")")" +echo $DIR +cd $DIR/.. +echo `pwd` + +ROS_DISTRO=$1 +GIT_BRANCH=$2 +UBUNTU_VERSION=$3 +NOCACHE="--no-cache" +NOCACHE= + +ROOT_DIR=`realpath $DIR/../../..` + +echo "ros distro: $ROS_DISTRO" +echo "git branch: $GIT_BRANCH" +echo "ubuntu version: $UBUNTU_VERSION" +echo "root path: $ROOT_DIR" + +cd $ROOT_DIR +sudo docker build --build-arg ROS_DISTRO=$ROS_DISTRO --build-arg GIT_BRANCH=$GIT_BRANCH --build-arg UBUNTU_VERSION=$UBUNTU_VERSION -t ue_editor_rclue:$ROS_DISTRO -f $ROOT_DIR/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/Dockerfile . $NOCACHE diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/build_docker_humble.sh b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/build_docker_humble.sh new file mode 100755 index 000000000..ed0429b57 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/build_docker_humble.sh @@ -0,0 +1,4 @@ +#!/bin/sh +DIR="$(dirname "$(realpath "$0")")" +echo $DIR +$DIR/build_docker.sh humble humble jammy diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/fastdds_setup.bash b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/fastdds_setup.bash new file mode 100755 index 000000000..f661c4240 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/fastdds_setup.bash @@ -0,0 +1,5 @@ +#!/bin/bash + +export RMW_IMPLEMENTATION=rmw_fastrtps_cpp +export ROS_DISCOVERY_SERVER="127.0.0.1:11811" +export FASTRTPS_DEFAULT_PROFILES_FILE=${PWD}/fastdds_config.xml diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/frames_2023-08-05_01.06.53.gv b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/frames_2023-08-05_01.06.53.gv new file mode 100644 index 000000000..52e51c916 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/frames_2023-08-05_01.06.53.gv @@ -0,0 +1,18 @@ +digraph G { +"map" -> "odom"[label=" Broadcaster: default_authority\nAverage rate: 50.4\nBuffer length: 5.0\nMost recent transform: 2384.550098\nOldest transform: 2379.550098\n"]; +"base_footprint" -> "base_link"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"]; +"odom" -> "base_footprint"[label=" Broadcaster: default_authority\nAverage rate: 30.119\nBuffer length: 4.98\nMost recent transform: 2384.390137\nOldest transform: 2379.409912\n"]; +"camera_link" -> "camera_depth_frame"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"]; +"base_link" -> "camera_link"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"]; +"camera_depth_frame" -> "camera_depth_optical_frame"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"]; +"camera_link" -> "camera_rgb_frame"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"]; +"camera_rgb_frame" -> "camera_rgb_optical_frame"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"]; +"base_link" -> "caster_back_left_link"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"]; +"base_link" -> "caster_back_right_link"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"]; +"base_link" -> "imu_link"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"]; +"base_link" -> "base_scan"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"]; +edge [style=invis]; + subgraph cluster_legend { style=bold; color=black; label ="view_frames Result"; +"Recorded at time: 1691190413.1841352"[ shape=plaintext ] ; +}->"map"; +} \ No newline at end of file diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/frames_2023-08-05_01.06.53.pdf b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/frames_2023-08-05_01.06.53.pdf new file mode 100644 index 0000000000000000000000000000000000000000..3b87381c1b28cca1cf68714ca49ae9a777c171d6 GIT binary patch literal 16806 zcmd74WmFy67A;Idf=h6B_j7P3xVyU@+}$Aw8rF(S2_PyhM zt42t$=uUq3wEy@u!LC0BGUQW(ozKWg2KkwDY*iQ{3x z=z8iT!Y=R7w(aKfu&~RPqkQ>9Uu)sJC*0SOMg{NrP~)FLI>tMFaai6T8i~{qG8!^^ zGsfDFA6;4TqPwbuj{RBNV5&QIQzC_`iz8%UO|&Wu4sz@~9c$Z;a)OD~(yqlQa#_CR z#^bmSUy{209Ly7+*)@~1tk`$dUQw1R(uhgg&yxw$fQd^h{)F@xq+0W%KhOmdKbZeB z$UWz^TLIJ}#&PFQQx9;&8SpHtvv3spEgm8={nEo^4YM`iTnwq;sf@}DRUN;XhjzjK zS-FA4O!BbS5#9_v2e>HS`RJ;MQ2=anqU-+lxbT!eQ7!Xkfq4VtLNj#>X>AHU8GP8g z;qrkRJCsqKBeYA;`EGbk<{h^PtEYA@Irp}4tWi}HQ?zghJ=uvX>^VtjZf9t1j)Tn0 zLOjwjq{2YipYXU*%hxGL8mMvFM{P~q1ayI9PJ%+B_`6I4)~*p*bX3jXlQq^)7_M58 z7W4gVHK>SuyDlA_Uwv)It25zlG!wu+3^u`*u5V?L&1WfrU~?`3^ds?o0u7yuXT{Cf zZXJ0pKxQ-zg%BK%+T0~D++nPi^Lg>ErdvJU6#2L{HMPc0dfYx0`&<_LJOmzBvnii$ zOa9bpTOZmTaq$QzFIoIpdAKxksLXoDV|g{RZC(aaR8b+dvAen&_VSJC`axY2)qBd6 zU}?L(7Ot5Z!Tt7luxO7D-U_|CEZn(&V2a9x zbSdsbMjE_F{!<7hS%=>^MzOIBUigssVGCfJrN_mSw&=~8flrmRqlUuLV) zs_$JFaf1EN3uPrpIxTJ5gkxEt6OEW5Dmy)vVJLd1SWgk8BBS!wvMX7gq|2C3GWW&p z4XSIshWRr8iq<_?k=Ux7lJHh8mvIkujn6d$=W@H*VM*6*b-pq@4z97TC< z9JN;QSAt9$B6tFl-JePzX&(>-=^lB9<@V~n%2|A)7nF?P5KNiez%p<3>Vli;8X}zX z&h>f~kjxW6y{Nzu$|xPR`erwsT$(~<bG)yL{KX@)TlLkx zBh!KJg2dVJ+$&Ip&Eq0dfC}D;K*o^+Hu>n45OL8?k`QehCpIQR{oL9V!14U`&06Yg82Z+4-|xX%x(NY(x#KAs%; z(l|@qWT$UA#ehh_d&N3??CD7bfuwT7ss)AV?Rffa+_(a=WM5TLj=-tb4%?~J;UO;3 zB+++l(5<2dB`m-(k4h=zh%fAh9=?03BA^>)a18w~ z8&$<XnlEFmcB@nvzsh=8knkd-cA)I5!AQfqns4$W1ql67vbP7^6GioAB zQD%A3YhNola!?XIN_DbAt+6rvc23UbF_LF1g^G4~ugi~|p$BgwJ!5lg`T~pZhx1uD zAE)ub&EDW+rKCDShQUt0W5@Gd#lbYCs$=cKzKC^*Y~B%q33xBDkBAK)z#&DL7pK4X z9vtQ1Q;q2R!6O;02G)0DuSVeXB4dZ=v)_K^Qj?{O3$hm2%~}zN-_BvLy6B#)_>?z5 zOdwy?Su6fF1Zfw$ha6ag?v|8KWUAB@ke3)n6poZOr3N5r>}4c=pmFfq7Z5Y> zY@@j;x_b(H6K{Tw97~IoAacx)_C$s~*s+3&bq7mzM;2%4VGYH6WId*gqFJ z;fR5V(!AvY+`bM7{dBI@FV*kf5tzv+jduI5QVA(TOk=!?sOJp zk4$p}7a1+-FithXQH0X@u6mwI+U5WZQ@V-;B*R+ORbGSwvox;aJl!%p*itO4Y-fFH zFR=%TmpfZ2U~y|9OKrI3+hufnJ4M=)8=aXuRL2hHw{cBO-bHC-oteKt)fUH3bl{}k z%`Bl9g0x~r-@f||oOw+}umUTxW>VfVdj%t?OhJQ!w}kSJ1F>KlS!l&LVu&+wcwvjD zSh7mIKCRNNYg_e`G)WXbQD$`Ra<(`Wl((}ljEdV5h5Hw#t>W-{EX&ts_>!G!xkmlP zaYp#|+Wt_7aBZa+hNdKwSIr}Di!1_^>Y9=w-shEOrGoCqGOo2OkNQu|T*s(>JlI@2 z-=)v=qfvQW-FwrdmZV_}$V+Y-@|%B#L!%X3F*#8!scuX$c5JA0j= zNQ|s3PQL2#eY5JVl)q*SU0PxWl`K|{=!!XXhIn@17X8H$!<2)p1rcY>!-2KC$9JJo z6pjog4!pDXIk4;{a!@ZG}lWEQJ zGjR?%;)V3Y7PBxp0NKkzS$giZ*Fd{_+M2yuVlvr6Z#na-U4>ZQZ)2k{fnzH2UBT@66?AO#duR$V*4c zi=2%vUGYL(AKYBxG9~*4xLustVT#VgdoVHK5gUn9H3WaoA8CQfk|kFljeOaVehSa@ zlbg`f-QDl7Gar&NV$=XTsi^%_Or%y&4j(k$kN<>IonDAMl{8J`$1^qb%&yx7UNpQssYj{B_5h@z$SRA_+$-*)CQ=n8I+QP?*s)YE&gWdX6&A z^FvO&{nG5nY3Ik9Rp^}9`Fh^|pAN|GnT_nw@D#!6l((r=#mvQgG(0DM)ROB`e%x$6 z5o3W`h+m0Ln|H*0n$?WsV++Q)5@Zwj2+t1phi_DkFQcoi0?m4h?v>|i8%hZ1n$j4e zA15Dh;J;)tz!+JB{(!~LcQ1hY1x3GHpK&xJJ0tr`ftMuHUwHc$!WMRQ6jgM5M%RSg z+%JabgFzqgoFHTn*CMpXPqR3oqrH>SpDDrTQbI<~W`;%zq5?0*KjyDsesw9%@Q+3&$EBhj&_6R@QKEOkb_QAo<^+RiFqP zGGk}=W>X=dz2g_&g9ix3Oy9i?gP4fcL$**%p(dY*?xqBinpxLL*Upk#C5zEtz3cE? zHF;2YopOG}^Iht{j1R^L`ydU;x4IEsPAALO;# zkR(PtDf3q zm-^@g7B6Qa3MSo_8jhqV&Sor=bL8|N!l5C=it3|1{*7R4DVoD&TkPpS{M9()?O=OCnyfeIn`GlvyFLhNh)pf89fUjIB}s$kn~XLFRi&|I8f1pdEimsDem% zasfO+RLr40=X|#xDqSDe@QA#gfQ|GZE~lA+@pkT$UF&s4Sy^Lh5q@$8AG5=H_pt&E zb$_YE*KjY@-6ls~`%E=vCS=*uQO<4x5O!%g#082|K^=QIMyd{KNSlIts%H)j&hRb1 zT0>1iiR3KT*1oxpiV*xy;&ymm`&ZhX7=^H-uvJn>Qmi|UjrP@|p~F=h-EU&KaJN7t z)|9w4xI7I%h6*xDA3po|{M?@_-_0ehWU6@2)tlA3e9_WgD% zhM~j#ip*9hk|XGy2fAnPM=s)PO%3Ki@_MLjaOVv=Vl50fn=EMII$Q2Q&3D^wGC0TM zrK}~Ta(k{RBb^=PQOCVp%lD-DTXy zojzURpVSy6d^hoW(^4gQVJ^;2n{UqHnwnU!wPCidy@=QiS&Q7Nq*Yu7kx)KWA3~3A zmsnugEyBc2QAuoqR8}e|$QVx*W?DPkUpn06mXz>T*-?#KB_W?fL5^AmMWI_rvN^S! z)PgE}!W1t3y)+@p4)pLF=htk>1SFfE4NG$@bEYRW(hXsfLn+Q<{HfMahqPyl>Wj{l zjEXsxQ`Q?X(AHiTe?T37EA>jV6Yb<}7p+}*o?KeVh%;&-NRvUk zni^@m+F-44xWOE@lCMRPyMpW-W1S_PYM z7i-lwW$;rHJvwX-5h!6_v9Ftq@f{p~1Ue-yxYI3{9mi9vYJq%SN1Q6Va$|0Lf+X>9 zv_L}|-%z&u*SoyfyF;%caurRP@{!_h(xC;^F}q%kL(-$83VA^pL@L!FpavF`P^H2a z8Q?{%?*t^8LkU5yT4GqTgD=o(L4|#Ab);73z3H7FJs3@|!aH;7u?^W@n5((m@9xUdLNs{mxBij6_i+Whd3! z15p3BBHfM~f6oWHH;w$6>5%ZKS#iOv-x z@0jKyd;FM3V*5YTp?-DzvizM2qL^%3q-W^feQCR_MD@^|E1LkzrSWXjrb742bh|VS zeiR03M)F+&!|AZ>+^jNBHBQFvL#Dw@EtLtv)5V2$U5W&{wv6PR>9}YUkkH*M=q z7>g=1fQ`=b5H^}Vild#z$UGkK%G&Tt#J9EnwcZc5AMB8Z{0`nOC@*+aYt|%b#^V`l zIAhip8j)z^Cz=g7nz+*kVbEeMV{jiX2q3ZM5meR}Mf#`W`wHr)r^=9e;$;Lr*nX=gJd zTuj)#HpqB4Q=^59#7ym*ciGak-{I%4Vx}8{T5+`VH{dp3-_6v4vZVBq7UWyy_+Bcwv zuOFA5FT774p|beXO5MX3QFPqpTC0a+-L&(wbr|>k9@_8@E}h1vIjc2b4OMnbsTIUn4Onlv9fe-R~IVR2S}k z-td^{bgD|@Vu1&wzHO3u9~Cq>dpg5hoMVgW&yH+d-Ay@hP>>YaOWTkKoJfs^qT|fl zGP;aI;9~X&1vJ>4Chk=)ed~eW#1@?wZTlJp?>LY()GKu-bRVYyTc%J%8SdH<8o&qE z)JsfA5MHJJOXdOoy3;o7`+5Gk59;y<(G@0bm_bFsAJxeahoOvr;1h1kKxPkK6R=nM zJbldf(%scvOSG7?4Sn2-?QnM1X_;NPnd|B7bxJLJ5}CWao_KHF4%5B`rs6G2+9Nz9 z^U2_9>Hdx*Po_k~Nvq0=A}=6~5&GlMR>@4sbV&)z1zD68_c~fBda^-qm>qy>rpQ>v`kv=>K2>` zH93JiWzXxQNr&V6Em;DeE7su@%*NhWu<}!}SD+MJ=pF~q~OC&wfBt;uAiiS%f`j# zeba-X%cEwq_@uC^d=Fj3@-Rja)i3a+Ne(%fhZ>e>D~dtD5+|n7^yf8i+a7X&?DP5h{2a)T-2P zYfR_g&bV7q=k$_eQth#0dh zX1`Jd z&xaPm(Xzu{f_874zvS_Mt*j5BJos(0stFKTjLqpJF7~=b5;<)!x4Nia$wLh*yIRBv z6BUdV!2EScEOL|ml!#iqV}?NXGI;Bwog9VWl}?lc&NTiK>0 z$iANOq@v^lk~gY|QB*3RRXk?nH7Wee&E%EEeN1ZIN10^;a2AvYzX}e){=j#=#Z=t`kN|AiEK_CES3ekT^|5!7%>^#y+;#%8$^>5(R~d+$eOT!IzNC*c9Bc!A zsKr_EV{!7T%fQLuVG7bZi;aFR1!6oUb+_s1yrUr3Q$gpxBs@U#y?3p`>H^}%AvrlN zBzNgsuDdUT1Adn?u9E6*bxM1bMrE#e8X!7^pQsX{qSh)wszRS(z?cQ}E-+3RGe6yL zI~96g$jsytbm97ze&cIXPIllU?GPGMF zwObMxtr{G+PMU#g!!mp*>Ye(TvC7QI1XVgT6^Gfa6ReCaM9kmV;|JU-@S3ku^r;n* z?ncn5?qs-~BI2Rd=&^peX*iGMHZ_;@p;uLR|0yob7sZ+F@#^v%gE*#eKj5L^gA8_} zjg*wk8yPusBgvUO2nsM*s8Uc<4|J$GISq%VGDwdkmbj$zEh-5I=2Hqn!96sktBv0ew;k1NG|1y2e#+My{YFk1d87xrEikBK_gi(4%Hu*cc^fIxYM zMK!@ufC@hG5k7-73SaD#IV}f(!z^l=WSn*>jjuv|1O7*ONUKEjDoC0uxPV~wc;+S?>n>uVA{gy54;$3X&+Q(1x zhF^5JVOIUk;_c;&)}hQ}T`5?UH{m>JUb9Av05C*%%@HokWum6q{8ul^^PM|Bd)?01 z*s`5od1lq1vAc0JO^~SU5x{Tr1ep+MU||g|;OZn_n6z&{oex9S&HC`2ay)T;u4tSU z^8~4El{9R%wNdWERX*2U@BDrP z!-7Z|m#O}MMdpdh#|>riuZ8Nv2#* z)(AcstbOubaVKPp27Tt%@EsbOTk|_sr|Bo@=j0y)FG{eNyOZWCOjAvj8fHvZdl}?Z z27^;pIOmp}RYY4H9cR}9R<*MP<5shRVQ!<2AH!7IJqo*~&k;@%lHOt1nTn8_IuQ;8 zs3?-HC>R%6rL9-0e-rDrj&BHIRH5U&5eeis$pQ7N;kB4`W~qk>)AqugqVA*ip%zpj z$C?VDA3EyAkX5&v@y*usY+zKsyQdv z#aOm2ljf_&bz$~yc<2Pyj9NpqE`ps)2i_ncdFFXr)oJX}UrL&l$eT$}sZ*mNLU+Tq zcQYfARhK5zaSG)s2xilGfGZx0o~L_rL-Ljjw>d|;`yd(zt}QQ}T$QmT5d7TPuG+45 zxn#Jsne(hR$aB3P@THJ#pzr6kGZ>4fi&#=&4Evr{!P~N>)ah}9(Q()P{u|GoEWSC9 za{YCcsEeg1lzFffVPCoT@`%s|3A8xA!XB!`M&9H_-f54zIDW~XU#S#?fngdg4va=M z9FjF$5CE*dqr+OMtHDUhe#uTLzS}Q%`8$aS0xj6b(9NxQ9uJLBI;%V1eU}5Zr?o4i zzR^9ljt7PZ8xtS9wIZ7et(z;VWyhbJ17Bs+xW#9!t;`pj%ZhfbuJ7mwwAya@hwhH% zQ8aa1ZNK-FQNUSxHR{G=#)bwHi}&MDk}WdU&1zF4*%X11i`xZJ8=zp7O!XDh&qgDY z!E2KnfpHLi-6QWA4eq|J$~Z&eJ2bgJ_3q$XL%c;f8b7jK{$2~OMEdnEM#eT$xX?8c z2q#NqLV>6IXwrt*QgLY@&R62&Id*!dx`?mt{aEVleEku;#f88$J3WyXUbCY)7|ko> zu~sK%EI8t+-VsYCAB8@Z?`f$)*K7ZpoKIuMCGIlG879+KNSLPd00wchkFcr_#+ik~ zR7tjBa%1-`v@&!cN{~GBYC5H&02RbnENC6{4~vnntEKWE%UbSi*jid_STE_cnmlg? zg?R!`K&>WkafiFF*Uelx+g16pGA3pBDX1mU%e(CgWb{`Dg+nfzZkZT%GF0d^aC-)S z5C#yi>`xI0d-`Dx9d`djKtLFB@aSJl7tR`6)xHdqu=tdFgiz;;svjyPpqWJ zqrjQOPzUpvy?5HTw_^jIGb6=pKxs-8#2=weh$ke*5Tn@5tq)z83uguV|x zwU=`bE+Wysp|72M?!8xPR>Sw9rXQ{hybG}R>o(1m=8N9f#l!&`_0Brt9>#3M-mv3y zs2ReShJ(SZFR93;YOiYIO(3r2f$x(Wf2q5lJ?o!E`Rla(jGgyy z^ggmH_M`4*f1KeBDRK+}&f=oH`JT88B6;xjHhU0kwdBgD+2Lz{G*xxc_6f%YcYZ>? z!1iSmg84@G`qF4=3j_(L@B|}qY?}lv`X6u!-6(_P@2S-KR6KC2 z<e8po3)a>x? z-BBJ*nLK*27tSMa8N@z?xU{%dY&zwgm?8&wY{w@d?#vKX2o9n}>xm7t93 za8F6bjT}|9Ap1hq$Iatuigjn)rqI zdC5GoGXbNlW-%J4*rs5^k%~h)C;Kpkobs8yRIXaQaH~1fB6)9dS9Q;Y$?atD z8}>4vQxPmm*&G@tTtI_a8;|H06uUeA`(3j(@MFGtj~sEnj_bnj{ygaSLPmLycE^x% zv1yChK64mkl+#pH^td6Cot?fJ z?D80E04kZ)D`8mko=Wum*=os`jKt&7$VQTlQ+6}360wvz8rnr6V8VoAmLVZK0GWjp zE-WmE=BAeJR8LJt8G6~eEwd#)z7*@A_jBg^Bs8NLX|KoKzR!WBO|wlE@b}oENqp3} z2U`h_=<8dhEgOFd%1*b?Lp|{nQe@I6hyd+W<-ivr4*L5yLh$(!&#Hgq*E^Q$X5eZc>jC=-yIk|Jr_HXi zC;MG14^-)5$YduqG$j&82iu@;t&eNN>d%KzH_$_Yjh^0yJj_}SjJqCDL~|$XJBhbx zPymOw7vPSVCt?Du8=~~K+m8unk3P3wh&A3OTL>Nt+`S7w5o=s14(+p>guAse&w~9T;y^ye|oIJd6G+SR%r_ zv;z1QAL@geK=n4`h$Cz`UTDK)@9(}AzTV^pm*qn~>@#ykry@6Md`Fsl0%SyUXUl?V z#?6v9o)UU4uIsoBNYNfKOPLDVO96BzPEBav2WhpuDmzWA~`UepoS@?j~AYif(r9HHS8B-4miyLUL4C|9SkMP%CofL$H|Z7 zNCJ$LPqnto+Htm7*f*x7VT zOSNo^;aHH?I3}fVv~NwnSD+YM7+?rjPts-Y2h!ZQvAWg!-P5My4v>#=Yj<(y#1u&O zD~3657lG^>ML-A?G4Z>b;z``VSZ13rlVqo}7(vE!6ao&?7+-1UZ9PV_Pj=GIlc-sU zoner;o1G%f6EfH&7aIehr-+)w2hN4-#NGo*@`%GaV1(Ib6)Wy%sEZXI;+i>;t9pIZ zrjQkqXr4nyL1mt)6PSgEO8J4i1WpGsd{Vq#IjS6g{H1YZp2Dxa2qR0%vN&j4CQM3x03-n-?~A+_$BxkzCzp;*630$*4fIcsMV`ibaBEz{R>LEe@L?d4#`slof$Z$^6-4PXzA@1pSMHbEextkox zMhbl+P1<0s2&mqjgTmH2U51h%m5KnlQA&y+ZHSAo9yFdTqJ&w{E`Tamk&S}od?rNv zKs zxkE<@Pvj&>U0L}#zgRVbrs%mhpB4~rJ9s|&+RDd;~Ge84Y(e^N-5+%@vj{?rq=-TFAO{MD|!` zNz$wJkV~>tUuvS_%HC?893ieznQozGKW?GuTZtw zk()B}4!oCf`Xr+vSV%OXty%YG7y4MI?bQzmo;gw1oNc@_8J1T5Hhk{(v&>$LFhN;W zH{0?#2eU$rPn)`tJ`?GQJUa&n_vL4X&J{H%^I{87aJg`JhUl~0m||GmXFBHyV_$Y1 z2rHwik>(4lKL*AIb_&KpGI6FwbqWq{O!Uq1>g5k|6Z4tlAB%mW|GH7Oud(y!eW2MI zM_i(k(0Y_;J(qg3!KIdbhN z_#%1Yc-CmWjOb;6FKIePCYE0YL48{>BQq1zXI0d5GrvrVjz(50gzUfTza+%|OuS6- zd5aq{626F${w_{>QE2@sPWmm0dMW$6IVS_hUs5ckXZh1h-Y=_wzJt*(t=Io_jhVfJ zqoAq2{htny(*Gk3Wc;n>`geyD{#ugXA}wL-=U@&qYZF2SRWoaTYX`GG4ZpfpH3K=C zIy_6hn1C-Tvj27d&Shm{CH(*M%k*6C?=7)0G7_>ez39gNc>)29FXAqC;LA$}K*+}O zvYGz-$pUz>vv52Qg8BK%`mBX}xpDvrnb>~cvobS3PvoT~*5|$Oa>v2U{#=UrInDU1 zG=Pwqjh&DM$VA8t0RGBhVPz&{XL-@^z1;oMx;<;wY?Q6dUV>lPIEU7L>~$?x4O)Nu&yqupWjVU*)~k2edNh*3!DJ*Gvy^E`@Hs3tApPOq zQte2)Bq^o~u>P`Kvxd57s4>INc(vH-q1sm<)CG8TWuvy#;nuQM5t@{t0;Mgaq$RS0 zt7?L_F(T!rtyXTqYCe}&*yO%pT?VjTZnI-sbH35?&KSaoG60c~hc3(8SBM_nFENIVC-X)-OG{U~Us(a8I7Nd1CVXg!u4q<*TpfmGwFArE%W%T=$H(To zwK^f?I(&%X_W>AB+N;Xm7SyB=yV<%-F}9j(&y`!eOJei3Wwru zHy>D9=KD>OQ3)zN`xi;FHA^u(dcgKwx<_jtf@|~q=m-?4uLPr}2RUS9KdXFYZvM$} z?lxPHwb}}tg+3S(>lnZn)EL`6Lo>g!t-O=Da(cef~KY`^UhCV12J99r~eK{?V90 z;PAb#0s0)ycL|J>X~oNpYXh(%Ho5nz-NgIElKI9}B0UOMM?zv;hE%9{#KC#Pm5Ek? z-3Eswqr}|npZrN3rIp;n{El=nWfCLm+U-)O{V zeun;O1-;8~bHT^KS5|CkZFWxX%~P>UwBH&|USC&Vl`LRseAk*!{pg!}WG;MmdQ?WE zV$60gjJl*yK=0jPIXLK3s=MqC_}XZ6M|zpwghGec*)hwjOzBs05~4}+&|4OG#Sln; z)4e0zK36pnd+Qy#HN_<4&mL#Y)NSVdLx8iEk11Z9$JpCm`@0ugxi(#%g}xTk){Mga zD2)w!}AVoMCi#~m<=pnniI=(ta9*H3PeW))&i|u@D zpc)r5_!u5;_77A9eA5D7L3Yd-$g8{E_gn~x_Iz`7s}Wuv+O-u>chNZsBl1mi{yeGf zvY0L4uFkmaIDJeA-Cg>@RNUVz#;4x8QYQ<`zC{}8DoY7!k3NCBMYp2^&b4 zx=5@}QFLDIL(vwz@Dorq;c|k{q9NxZ@!qo(EubRtF5SwkyGBAVl%`jqDOK1}Q#0F3xoaLQN6vCRlN!6*t^ zC-3o7nifRjd|8BpSwzBZ$ykJYV}3?8?w7o$RXiX%Vx+DOD%jIf>H{f@Bo?n1mY&EN zF;O>uhC!s3;_#QuKo+^#1dAK{0;0FH2QzAorSdd_xF7XAYsjf34V2K7?^=46>lzG- zv;J(o*|Z33m=|90bY&PakWo1hu#-YxkOvKRAxjPS`YD7mK#l)YK?PXwgWbTmr83Lv zr+(Cm2({>i6D)wjcaV)-K`uRTG z?lX<1A5y{l;-(LR19mhc)v3%;m3-UI~&ud@{=N$?7-HvH+K%6R?y!S?zWKzL;@ul(HF-3 zig_fR#@&l#h?;zEWx`e-G0Ttg8$~|`dt_jgd!AtpmJdf4SZ6~}A-(?YpiY%9Fmn#8 zC}P|9U%dDyD1F8vjEw)Ha4(?mpIG&OLU&0~aYbdt|10Ru{0ncs;MD(jkozwz_iy0t z?|}vX3%d)73%yL^CHprW`4@iwAO3l+q3GuD%pSz8jcs21@UmwWj7-d)$?!AV;eU=7 zG5SSwUXY@!J?NPeJp*=1anLh8F>`eLD@)PI*4EO<>V>rYVkCdWN&F}H|0^DZL6uMw z@C@!*pOG`$OC*Q(-(o2K`_lhfCB?sJ%JUq5bD#g?9RH*hgbaTUn@;RkW4~z@NdK8G zJ$vniYXE;Srr%*De?!{8qtX8r!1D5^gn!?gF99qLzZ#V^1HH8LLKYPM3Q%!+iD&uS z41Y0!KU/dev/null 2>&1; then + echo "Package $package_name is installed." + echo "Checking if it has version $package_version." + + installed_package_description=$(apt list --installed | grep -w "$package_name") + echo "Installed package: $installed_package_description" + + if [[ $installed_package_description =~ $regex ]]; then + installed_version="${BASH_REMATCH[2]}" + fi + echo "Installed version: $installed_version" + + # Verify if the installed version matches the desired version + if [[ "$installed_version" == "$package_version" ]]; then + echo "Package $package_name-$package_version is already installed." + else + echo "WARNING: Package $package_name is installed, but it does not have the same debian package version $package_version." + DO_INSTALL=1 + fi +else + echo "Package $package_name is not installed." + DO_INSTALL=1 +fi + +if [ "$DO_INSTALL" == "1" ]; then + echo "Attempting to install package $package_name-$package_version." + # Attempt to install the package + sudo apt-get install -y "$package_name" + + # Check if the installation was successful + if dpkg -s "$package_name" >/dev/null 2>&1; then + echo "Package $package_name-$package_version has been successfully installed." + else + echo "Unable to install package $package_name-$package_version." + fi +else + echo "Checking nvidia-driver. Nothing to do. Package $package_name-$package_version is already installed." +fi diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/openvpn/login.txt b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/openvpn/login.txt new file mode 100644 index 000000000..e69de29bb diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/openvpn/ovpnconfig.ovpn b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/openvpn/ovpnconfig.ovpn new file mode 100644 index 000000000..e69de29bb diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/openvpn/password.conf b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/openvpn/password.conf new file mode 100644 index 000000000..e69de29bb diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/openvpn_file_run.sh b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/openvpn_file_run.sh new file mode 100755 index 000000000..0d35527a9 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/openvpn_file_run.sh @@ -0,0 +1,7 @@ +#!/bin/sh +xhost + + +DIR="$(dirname "$(realpath "$0")")" +ROOT_DIR=`realpath $DIR/../../../..` + +sudo nvidia-docker run --net host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name smacc_ue -v $ROOT_DIR/SMACC2:/home/ros2_ws/src/SMACC2 -v $ROOT_DIR/SMACC2_RTA:/home/ros2_ws/src/SMACC2_RTA -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/remove_container.sh b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/remove_container.sh new file mode 100755 index 000000000..1247aae92 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/remove_container.sh @@ -0,0 +1 @@ +sudo docker rm smacc_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/run_docker_container_bash.sh b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/run_docker_container_bash.sh new file mode 100755 index 000000000..5ab294c08 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/run_docker_container_bash.sh @@ -0,0 +1,8 @@ +#!/bin/sh +xhost + + +DIR="$(dirname "$(realpath "$0")")" +ROOT_DIR=`realpath $DIR/../../../..` +DRIVER=$(apt list --installed | grep nvidia-driver) + +sudo nvidia-docker run --net host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name smacc_ue -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash -c "./nvidia-driver-check.sh '$DRIVER' && bash" diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/run_docker_container_bash_development.sh b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/run_docker_container_bash_development.sh new file mode 100755 index 000000000..c446c50a5 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/run_docker_container_bash_development.sh @@ -0,0 +1,8 @@ +#!/bin/sh +xhost + + +DIR="$(dirname "$(realpath "$0")")" +ROOT_DIR=`realpath $DIR/../../../..` +DRIVER=$(apt list --installed | grep nvidia-driver) + +sudo nvidia-docker run --net host --ipc=host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name smacc_ue -v $ROOT_DIR/ue_project_1:/home/ros2_ws/src/ue_project_1 -v $ROOT_DIR/SMACC2:/home/ros2_ws/src/SMACC2 -v $ROOT_DIR/UE-Plugins:/home/ros2_ws/src/ue_project_1/Plugins/RapyutaSimulationPlugins -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash -c "./nvidia-driver-check.sh '$DRIVER' && bash" diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/run_docker_container_editor.sh b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/run_docker_container_editor.sh new file mode 100755 index 000000000..2e93b8c7e --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/run_docker_container_editor.sh @@ -0,0 +1,8 @@ +#!/bin/sh +xhost + + +DIR="$(dirname "$(realpath "$0")")" +ROOT_DIR=`realpath $DIR/../../../..` + +DRIVER=$(apt list --installed | grep nvidia-driver) +sudo nvidia-docker run --net host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name smacc_ue -v $ROOT_DIR/ue_project_2:/home/ros2_ws/src/ue_project_2 -v $ROOT_DIR/ue_project_1:/home/ros2_ws/src/ue_project_1 -v $ROOT_DIR/SMACC2:/home/ros2_ws/src/SMACC2 -v $ROOT_DIR/UE-Plugins:/home/ros2_ws/src/turtlebot3-UE/Plugins/RapyutaSimulationPlugins -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash -c "./nvidia-driver-check.sh '$DRIVER' && ./run_editor_smacc.sh" diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/run_editor_smacc.sh b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/run_editor_smacc.sh new file mode 100755 index 000000000..aa13d0389 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/run_editor_smacc.sh @@ -0,0 +1,32 @@ +#!/bin/bash +# Copyright 2020-2022 Rapyuta Robotics Co., Ltd. + +export LD_LIBRARY_PATH=$(echo "$LD_LIBRARY_PATH" | sed 's#/opt/ros/humble/lib:##') +echo "UPDATED LD_LIBRARY_PATH: $LD_LIBRARY_PATH" + +if [ -z "${UE5_DIR}" ]; then + printf "Please set UE5_DIR to path of UE5 folder\n" + exit 1 +fi + +DISCOVERY_SERVER=${1:-true} +CURRENT_DIR=$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" &> /dev/null && pwd) +TB3_UE_DIR=${2:-"${CURRENT_DIR}"} +# if $DISCOVERY_SERVER; then +# # Run discovery service for FastDDS +# (exec "${TB3_UE_DIR}/run_discovery_service.sh") + +# # Configure environment for FastDDS discovery +# source ${TB3_UE_DIR}/fastdds_setup.sh +# fi + +#change default level, generating DefautlEngine.ini +DEFAULT_LEVEL=${LEVEL_NAME:-"Default"} +DEFAULT_RATE=${FIXED_FRAME_RATE:-"100.0"} +DEFAULT_RTF=${TARGET_RTF:-"1.0"} +sed -e 's/${LEVEL_NAME}/'${DEFAULT_LEVEL}'/g' Config/DefaultEngineBase.ini > Config/DefaultEngine.ini +sed -i -e 's/${FIXED_FRAME_RATE}/'${DEFAULT_RATE}'/g' Config/DefaultEngine.ini +sed -i -e 's/${TARGET_RTF}/'${DEFAULT_RTF}'/g' Config/DefaultEngine.ini + +UE_EDITOR="${UE5_DIR}/Engine/Binaries/Linux/UnrealEditor" +(exec "$UE_EDITOR" "${TB3_UE_DIR}/ue_project1.uproject" -norelativemousemode) diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/save_current_docker_image.sh b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/save_current_docker_image.sh new file mode 100755 index 000000000..3cd242149 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/save_current_docker_image.sh @@ -0,0 +1,11 @@ +#!/bin/bash + +OUTPUT_PATH=${1} + +if [ -z "$OUTPUT_PATH" ]; then + echo "Usage: $0 " + exit 1 +fi + +DATE_SUFFIX=`date +%Y%m%d_%H%M%S` +docker save -o "$OUTPUT_PATH/unreal_editor_smacc_$DATE_SUFFIX.tar" ue_editor_rclue:humble diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/start_container.sh b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/start_container.sh new file mode 100755 index 000000000..ec24e9d12 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/start_container.sh @@ -0,0 +1 @@ +sudo docker start smacc_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/stop_container.sh b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/stop_container.sh new file mode 100755 index 000000000..3290cdb0c --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/stop_container.sh @@ -0,0 +1 @@ +sudo docker stop smacc_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/update_project_files.sh b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/update_project_files.sh new file mode 100755 index 000000000..086a474a3 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/update_project_files.sh @@ -0,0 +1,2 @@ +#!/bin/bash +/home/ros2_ws/src/UE5.1/Engine/Build/BatchFiles/Linux/GenerateProjectFiles.sh: diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgalery_ue/states/st_back_on_road_waypoints_x.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgalery_ue/states/st_back_on_road_waypoints_x.hpp new file mode 100644 index 000000000..0130dfd1c --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgalery_ue/states/st_back_on_road_waypoints_x.hpp @@ -0,0 +1,68 @@ +// 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 + +namespace sm_dancebot_artgallery_ue +{ + +// STATE DECLARATION +struct StBackOnRoadWaypointsX : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // CUSTOM TRANSITION TAGS + struct TRANSITION_1 : SUCCESS{}; + struct TRANSITION_2 : SUCCESS{}; + struct TRANSITION_3 : SUCCESS{}; + struct TRANSITION_4 : SUCCESS{}; + struct TRANSITION_5 : SUCCESS{}; + struct TRANSITION_6 : SUCCESS{}; + + // TRANSITION TABLE + typedef mpl::list< + Transition, + Transition, StBackOnRoadWaypointsX, TRANSITION_1> + // Transition, StNavigateToWaypointsX, TRANSITION_2>, + // Transition, SS1::SsRadialPattern1, TRANSITION_3>, + // Transition, SS2::SsRadialPattern2, TRANSITION_4> + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + // configure_orthogonal(); + configure_orthogonal("waypoints_plan_back_on_road", "sm_dancebot_artgallery_ue"); + configure_orthogonal(); + } + + void onEntry() + { + } + + void runtimeConfigure() {} + + void onExit(ABORT) + { + } +}; +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgalery_ue/states/st_inital_road_waypoints_x.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgalery_ue/states/st_inital_road_waypoints_x.hpp new file mode 100644 index 000000000..f838c0637 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgalery_ue/states/st_inital_road_waypoints_x.hpp @@ -0,0 +1,64 @@ +// 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 + +namespace sm_dancebot_artgallery_ue +{ + +// STATE DECLARATION +struct StInitialRoadWaypointsX : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // CUSTOM TRANSITION TAGS + struct TRANSITION_1 : SUCCESS{}; + struct TRANSITION_2 : SUCCESS{}; + struct TRANSITION_3 : SUCCESS{}; + struct TRANSITION_4 : SUCCESS{}; + struct TRANSITION_5 : SUCCESS{}; + struct TRANSITION_6 : SUCCESS{}; + + // TRANSITION TABLE + typedef mpl::list< + Transition, + //Transition, StTurnAround, TRANSITION_1>, + Transition, StInitialRoadWaypointsX, TRANSITION_2> + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(); + } + + void onEntry() + { + } + + void runtimeConfigure() {} + + void onExit(ABORT) + { + } +}; +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/cl_nav2z/client_behaviors/cb_active_stop.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/cl_nav2z/client_behaviors/cb_active_stop.hpp new file mode 100644 index 000000000..077f8efcd --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/cl_nav2z/client_behaviors/cb_active_stop.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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include + +namespace sm_dancebot_artgallery_ue +{ +struct CbActiveStop : public smacc2::SmaccAsyncClientBehavior +{ +private: + + + rclcpp::Publisher::SharedPtr cmd_vel_pub_; + +public: + + CbActiveStop(); + + void onEntry() override; + + void onExit() override; +}; +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/cl_nav2z/client_behaviors/cb_load_waypoints_file.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/cl_nav2z/client_behaviors/cb_load_waypoints_file.hpp new file mode 100644 index 000000000..069f35d5c --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/cl_nav2z/client_behaviors/cb_load_waypoints_file.hpp @@ -0,0 +1,63 @@ +// 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 + +namespace sm_dancebot_artgallery_ue +{ +struct CbLoadWaypointsFile : public smacc2::SmaccClientBehavior +{ +public: + CbLoadWaypointsFile(std::string filepath) : filepath_(filepath) {} + + CbLoadWaypointsFile(std::string parameter_name, std::string packagenamesapce) + : + parameterName_(parameter_name), + packagenamesapce_(packagenamesapce) + { + } + + void onEntry() override + { + requiresComponent(waypointsNavigator_); // this is a component from the nav2z_client library + + if (filepath_) + this->waypointsNavigator_->loadWayPointsFromFile(filepath_.value()); + else + this->waypointsNavigator_->loadWaypointsFromYamlParameter( + parameterName_.value(), packagenamesapce_.value()); + + // change this to skip some points of the yaml file, default = 0 + waypointsNavigator_->currentWaypoint_ = 0; + } + + void onExit() override {} + + std::optional filepath_; + + std::optional parameterName_; + std::optional packagenamesapce_; + + cl_nav2z::CpWaypointNavigatorBase * waypointsNavigator_; +}; +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/cl_nav2z/client_behaviors/cb_navigate_next_waypoint_free.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/cl_nav2z/client_behaviors/cb_navigate_next_waypoint_free.hpp new file mode 100644 index 000000000..c2fb7df22 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/cl_nav2z/client_behaviors/cb_navigate_next_waypoint_free.hpp @@ -0,0 +1,60 @@ +// 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 + +namespace sm_dancebot_artgallery_ue +{ + +class CbNavigateNextWaypointFree : public sm_dancebot_artgallery_ue::CbPositionControlFreeSpace +{ +public: + CbNavigateNextWaypointFree() { } + + virtual ~CbNavigateNextWaypointFree() {} + + void onEntry() override + { + requiresComponent(this->waypointsNavigator_); + this->target_pose_ = this->waypointsNavigator_->getCurrentPose(); + + this->onSuccess(&CbNavigateNextWaypointFree::onSucessCallback, this); + RCLCPP_INFO_STREAM(getLogger(), "[CbNavigateNextWaypoint] initial load file target pose: x: " << this->target_pose_.position.x << ", y: " << this->target_pose_.position.y); + CbPositionControlFreeSpace::onEntry(); + } + + void onSucessCallback() + { + RCLCPP_INFO_STREAM(getLogger(), "[CbNavigateNextWaypoint] Success on planning to next waypoint"); + this->waypointsNavigator_->notifyGoalReached(); + this->waypointsNavigator_->forward(1); + RCLCPP_INFO_STREAM(getLogger(), "[CbNavigateNextWaypoint] next position index: " << this->waypointsNavigator_->getCurrentWaypointIndex() << "/" << this->waypointsNavigator_->getWaypoints().size()); + } + + void onExit() override + { + } + +protected: + cl_nav2z::CpWaypointNavigatorBase * waypointsNavigator_; +}; + +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/cl_nav2z/client_behaviors/cb_position_control_free_space.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/cl_nav2z/client_behaviors/cb_position_control_free_space.hpp new file mode 100644 index 000000000..43528ca56 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/cl_nav2z/client_behaviors/cb_position_control_free_space.hpp @@ -0,0 +1,65 @@ +// 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 sm_dancebot_artgallery_ue +{ +struct CbPositionControlFreeSpace : public smacc2::SmaccAsyncClientBehavior +{ +private: + double targetYaw_; + bool goalReached_; + double k_betta_; + double max_angular_yaw_speed_; + + double prev_error_linear_ = 0.0; + double prev_error_angular_ = 0.0; + double integral_linear_ = 0.0; + double integral_angular_ = 0.0; + + // Limit the maximum linear velocity and angular velocity to avoid sudden movements + double max_linear_velocity = 1.0; // Adjust this value according to your needs + double max_angular_velocity = 1.0; // Adjust this value according to your needs + + rclcpp::Publisher::SharedPtr cmd_vel_pub_; + +public: + double yaw_goal_tolerance_rads_=0.1; + + double threshold_distance_ = 3.0; + + geometry_msgs::msg::Pose target_pose_; + + CbPositionControlFreeSpace(); + + void updateParameters(); + + void onEntry() override; + + void onExit() override; +}; +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/cl_nav2z/client_behaviors/cb_pure_spinning.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/cl_nav2z/client_behaviors/cb_pure_spinning.hpp new file mode 100644 index 000000000..5d678298e --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/cl_nav2z/client_behaviors/cb_pure_spinning.hpp @@ -0,0 +1,126 @@ +// 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 + +namespace sm_dancebot_artgallery_ue +{ +struct CbPureSpinning : public smacc2::SmaccAsyncClientBehavior +{ + private: + double targetYaw__rads; + bool goalReached_; + double k_betta_; + double max_angular_yaw_speed_; + + rclcpp::Publisher::SharedPtr cmd_vel_pub_; + + public: + double yaw_goal_tolerance_rads_; + + CbPureSpinning(double targetYaw_rads, double max_angular_yaw_speed= 0.5) + : targetYaw__rads(targetYaw_rads), + k_betta_(1.0), + max_angular_yaw_speed_(max_angular_yaw_speed), + yaw_goal_tolerance_rads_(0.1) + { + + } + + void updateParameters() + { + } + + void onEntry() override + { + auto nh = this->getNode(); + cmd_vel_pub_ = nh->create_publisher("/cmd_vel", rclcpp::QoS(1)); + + sm_dancebot_artgallery_ue::CpUEPose* pose; + this->requiresComponent(pose); + + geometry_msgs::msg::Twist cmd_vel; + goalReached_ = false; + + geometry_msgs::msg::Pose currentPose = pose->toPoseMsg(); + + rclcpp::Rate loop_rate(10); + double countAngle = 0; + auto prevyaw = tf2::getYaw(currentPose.orientation); + while(rclcpp::ok() && !goalReached_) + { + tf2::Quaternion q; + currentPose = pose->toPoseMsg(); + tf2::fromMsg(currentPose.orientation, q); + auto currentYaw = tf2::getYaw(currentPose.orientation); + auto deltaAngle = angles::shortest_angular_distance(prevyaw, currentYaw); + countAngle += deltaAngle; + + prevyaw = currentYaw; + double angular_error = targetYaw__rads - countAngle ; + + auto omega = angular_error * k_betta_; + cmd_vel.linear.x = 0; + cmd_vel.linear.y = 0; + cmd_vel.linear.z = 0; + cmd_vel.angular.z = + std::min(std::max(omega, -fabs(max_angular_yaw_speed_)), fabs(max_angular_yaw_speed_)); + + RCLCPP_INFO_STREAM(getLogger(), "["<cmd_vel_pub_->publish(cmd_vel); + + loop_rate.sleep(); + } + + this->postSuccessEvent(); + } + + void onExit() override + { + } + }; +} \ No newline at end of file diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/components/cp_ue_pose.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/components/cp_ue_pose.hpp new file mode 100644 index 000000000..ea79a2c8b --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/components/cp_ue_pose.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. + +/***************************************************************************************************************** + *-2020 + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace sm_dancebot_artgallery_ue +{ + +class CpUEPose : public smacc2::components::CpTopicSubscriber +{ +public: + CpUEPose(std::string topicname); + + void onInitialize() override; + + void onPoseMessageReceived(const ue_msgs::msg::EntityState& msg); + + geometry_msgs::msg::Pose toPoseMsg(); + +private: + ue_msgs::msg::EntityState entityStateMsg_; + +}; +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/modestates/ms_dance_bot_recovery_mode.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/modestates/ms_dance_bot_recovery_mode.hpp new file mode 100644 index 000000000..2ce6116b8 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/modestates/ms_dance_bot_recovery_mode.hpp @@ -0,0 +1,38 @@ +// 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 + * + ******************************************************************************************************************/ + +#include +namespace sm_dancebot_artgallery_ue +{ +// STATE DECLARATION +class MsDanceBotRecoveryMode : public smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition> + + >reactions; + // typedef Transition reactions; +}; +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/modestates/ms_dance_bot_run_mode.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/modestates/ms_dance_bot_run_mode.hpp new file mode 100644 index 000000000..c7133b396 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/modestates/ms_dance_bot_run_mode.hpp @@ -0,0 +1,37 @@ +// 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 + * + ******************************************************************************************************************/ + +#include +namespace sm_dancebot_artgallery_ue +{ +// STATE DECLARATION +class MsDanceBotRunMode : public smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition + + >reactions; +}; +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/orthogonals/or_navigation.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/orthogonals/or_navigation.hpp new file mode 100644 index 000000000..494864314 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/orthogonals/or_navigation.hpp @@ -0,0 +1,71 @@ +// 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 +#include + +namespace sm_dancebot_artgallery_ue +{ +using namespace cl_nav2z; +using namespace std::chrono_literals; + +class OrNavigation : public smacc2::Orthogonal +{ +public: + void onInitialize() override + { + auto nav2zClient = this->createClient(); + + // create pose component + nav2zClient->createComponent(); + + // create planner switcher + nav2zClient->createComponent(); + + // create goal checker switcher + nav2zClient->createComponent(); + + // create odom tracker + nav2zClient->createComponent(); + + // create odom tracker + nav2zClient->createComponent(); + + // create waypoints navigator component + auto waypointsNavigator = nav2zClient->createComponent(); + + nav2zClient->createComponent("/ue_ros/map_origin_entity_state"); + + /*auto waypointsNavigator = */ + // nav2zClient->createComponent<::cl_nav2z::CpWaypointNavigatorBase>(); + } +}; +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/orthogonals/or_obstacle_perception.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/orthogonals/or_obstacle_perception.hpp new file mode 100644 index 000000000..04f327273 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/orthogonals/or_obstacle_perception.hpp @@ -0,0 +1,39 @@ +// 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 + +namespace sm_dancebot_artgallery_ue +{ + using namespace std::chrono_literals; +class OrObstaclePerception : public smacc2::Orthogonal +{ +public: + void onInitialize() override + { + // auto lidarClient = this->createClient(); + + // lidarClient->topicName = "/scan"; + // lidarClient->timeout_ = rclcpp::Duration(10s); + } +}; +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/sm_dancebot_artgallery_ue.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/sm_dancebot_artgallery_ue.hpp new file mode 100644 index 000000000..a0095f3d0 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/sm_dancebot_artgallery_ue.hpp @@ -0,0 +1,149 @@ +// 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 + * + ******************************************************************************************************************/ + +#include +#include + +#include + +// CLIENT BEHAVIORS +#include + +#include +#include + +// #include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +//STATE REACTORS +#include +#include +#include + + +// ORTHOGONALS +#include +#include + +using namespace cl_nav2z; +using namespace smacc2::state_reactors; + +namespace sm_dancebot_artgallery_ue +{ +//STATE FORWARD DECLARATIONS +class StAcquireSensors; +class StInitialRoadWaypointsX; +class StNavigateFieldWaypointsX; +class StBackOnRoadWaypointsX; +class StTurnAround; +class StFinalState; + +//SUPERSTATE FORWARD DECLARATIONS +//MODE STATES FORWARD DECLARATIONS +class MsDanceBotRunMode; +class MsDanceBotRecoveryMode; + +namespace SS1 +{ +class SsRadialPattern1; +} + +namespace SS2 +{ +class SsRadialPattern2; +} + +namespace SS3 +{ +class SsRadialPattern3; +} + +namespace SS4 +{ +class SsFPattern1; +} + +namespace SS5 +{ +class SsSPattern1; +} + +// custom smd_dance_bot event +struct EvGlobalError : sc::event +{ +}; + +} // namespace sm_dancebot_artgallery_ue + +using namespace sm_dancebot_artgallery_ue; +using namespace cl_ros_timer; +using namespace smacc2; + +namespace sm_dancebot_artgallery_ue +{ +/// \brief Advanced example of state machine with smacc that shows multiple techniques +/// for the development of state machines +struct SmDanceBot : public smacc2::SmaccStateMachineBase +{ + typedef mpl::bool_ shallow_history; + typedef mpl::bool_ deep_history; + typedef mpl::bool_ inherited_deep_history; + + using SmaccStateMachineBase::SmaccStateMachineBase; + + void onInitialize() override + { + this->createOrthogonal(); + this->createOrthogonal(); + } +}; + +} // namespace sm_dancebot_artgallery_ue + +//MODE STATES +#include + +#include + +//SUPERSTATES +#include +#include +#include +#include +#include + +//STATES +#include + +#include +#include +#include +#include +#include diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_forward_1.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_forward_1.hpp new file mode 100644 index 000000000..6913a3190 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_forward_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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +namespace sm_dancebot_artgallery_ue +{ +namespace f_pattern_states +{ +// STATE DECLARATION +template +struct StiFPatternForward1 : public smacc2::SmaccState, SS> +{ + typedef SmaccState, SS> TSti; + using TSti::context_type; + using TSti::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiFPatternReturn1>, + Transition, StiFPatternReturn1, ABORT> + + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + TSti::template configure_orthogonal(SS::ray_lenght_meters()); + TSti::template configure_orthogonal(); + } + + void runtimeConfigure() + { + // auto &superstate = TSti::template context(); + // RCLCPP_INFO(this->getLogger(),"[SsrFpattern] Fpattern rotate: SS current iteration: %d/%d", + // superstate.iteration_count, SS::total_iterations()); + } +}; +} // namespace f_pattern_states +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_forward_2.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_forward_2.hpp new file mode 100644 index 000000000..90be73344 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_forward_2.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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +namespace sm_dancebot_artgallery_ue +{ +namespace f_pattern_states +{ +// STATE DECLARATION +template +struct StiFPatternForward2 : smacc2::SmaccState, SS> +{ + typedef SmaccState, SS> TSti; + using TSti::context_type; + using TSti::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiFPatternStartLoop>, + Transition, StiFPatternStartLoop> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + TSti::template configure_orthogonal(SS::pitch_lenght_meters()); + TSti::template configure_orthogonal(); + } + + void runtimeConfigure() {} +}; +} // namespace f_pattern_states +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_loop_start.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_loop_start.hpp new file mode 100644 index 000000000..2ae634f00 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_loop_start.hpp @@ -0,0 +1,55 @@ +// 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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_artgallery_ue +{ +namespace f_pattern_states +{ +// STATE DECLARATION +template +struct StiFPatternStartLoop : smacc2::SmaccState, SS> +{ + typedef SmaccState, SS> TSti; + using TSti::context_type; + using TSti::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition>, StiFPatternRotate1, CONTINUELOOP> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + bool loopCondition() + { + auto & superstate = TSti::template context(); + return superstate.iteration_count++ < superstate.total_iterations(); + } + + void onEntry() + { + TSti::checkWhileLoopConditionAndThrowEvent(&StiFPatternStartLoop::loopCondition); + } +}; +} // namespace f_pattern_states +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_return_1.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_return_1.hpp new file mode 100644 index 000000000..2a847492a --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_return_1.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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +namespace sm_dancebot_artgallery_ue +{ +namespace f_pattern_states +{ +// STATE DECLARATION +template +struct StiFPatternReturn1 : smacc2::SmaccState, SS> +{ + typedef SmaccState, SS> TSti; + using TSti::context_type; + using TSti::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiFPatternRotate2> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + TSti::template configure_orthogonal(); + TSti::template configure_orthogonal(); + } + + void runtimeConfigure() {} +}; +} // namespace f_pattern_states +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_rotate_1.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_rotate_1.hpp new file mode 100644 index 000000000..927b54bf6 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_rotate_1.hpp @@ -0,0 +1,60 @@ +// 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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_artgallery_ue +{ +namespace f_pattern_states +{ +// STATE DECLARATION +template +struct StiFPatternRotate1 : smacc2::SmaccState, SS> +{ + typedef SmaccState, SS> TSti; + using TSti::context_type; + using TSti::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiFPatternForward1>, + Transition, StiFPatternRotate1> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + float angle = 0; + double offset = -1.5; // for a better behaving + + if (SS::direction() == TDirection::LEFT) + angle = 90 + offset; + else + angle = -90 - offset; + + //TSti::template configure_orthogonal(angle); + TSti::template configure_orthogonal( + angle); // absolute aligned to the y-axis + } + + void runtimeConfigure() {} +}; +} // namespace f_pattern_states +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_rotate_2.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_rotate_2.hpp new file mode 100644 index 000000000..e13928efe --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_rotate_2.hpp @@ -0,0 +1,62 @@ +// 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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_artgallery_ue +{ +namespace f_pattern_states +{ +// STATE DECLARATION +template +struct StiFPatternRotate2 : smacc2::SmaccState, SS> +{ + typedef SmaccState, SS> TSti; + using TSti::context_type; + using TSti::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiFPatternForward2>, + Transition, StiFPatternRotate2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + double offset = 1.5; // for a better behaving + // float angle = 0; + + // if (SS::direction() == TDirection::LEFT) + // angle = -90 - offset; + // else + // angle = 90 + offset; + + //TSti::template configure_orthogonal(angle); + TSti::template configure_orthogonal( + 0 + offset); // absolute horizontal + + TSti::template configure_orthogonal(); + } + + void runtimeConfigure() {} +}; +} // namespace f_pattern_states +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/radial_motion_states/sti_radial_end_point.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/radial_motion_states/sti_radial_end_point.hpp new file mode 100644 index 000000000..73af3a043 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/radial_motion_states/sti_radial_end_point.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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +namespace sm_dancebot_artgallery_ue +{ +namespace radial_motion_states +{ +// STATE DECLARATION +struct StiRadialEndPoint : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiRadialReturn, SUCCESS>, + Transition, StiRadialReturn, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + //RCLCPP_INFO(getLogger(),"ssr radial end point, distance in meters: %lf", SS::ray_length_meters()); + configure_orthogonal(SS::ray_length_meters()); + configure_orthogonal(); + } + + void runtimeConfigure() {} +}; +} // namespace radial_motion_states +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/radial_motion_states/sti_radial_loop_start.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/radial_motion_states/sti_radial_loop_start.hpp new file mode 100644 index 000000000..4c5d5acca --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/radial_motion_states/sti_radial_loop_start.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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +namespace sm_dancebot_artgallery_ue +{ +namespace radial_motion_states +{ +// STATE DECLARATION +struct StiRadialLoopStart : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiRadialRotate, 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(&StiRadialLoopStart::loopWhileCondition); + } +}; +} // namespace radial_motion_states +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/radial_motion_states/sti_radial_return.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/radial_motion_states/sti_radial_return.hpp new file mode 100644 index 000000000..8169a6885 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/radial_motion_states/sti_radial_return.hpp @@ -0,0 +1,55 @@ +// 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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_artgallery_ue +{ +namespace radial_motion_states +{ +// STATE DECLARATION +struct StiRadialReturn : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiRadialLoopStart, SUCCESS>, + Transition, StiRadialEndPoint, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(); + configure_orthogonal(); + } + + void onExit() + { + ClNav2Z * moveBase; + this->requiresClient(moveBase); + + auto odomTracker = moveBase->getComponent(); + odomTracker->clearPath(); + } +}; +} // namespace radial_motion_states +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/radial_motion_states/sti_radial_rotate.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/radial_motion_states/sti_radial_rotate.hpp new file mode 100644 index 000000000..8f5d77a6f --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/radial_motion_states/sti_radial_rotate.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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +namespace sm_dancebot_artgallery_ue +{ +namespace radial_motion_states +{ +// STATE DECLARATION +struct StiRadialRotate : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiRadialEndPoint, SUCCESS>, + Transition, StiRadialRotate, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() + { + auto cbAbsRotate = this->getClientBehavior(); + + cbAbsRotate->spinningPlanner = SpinningPlanner::PureSpinning; + + auto & superstate = this->context(); + cbAbsRotate->absoluteGoalAngleDegree = + superstate.iteration_count * SS::ray_angle_increment_degree(); + } +}; +} // namespace radial_motion_states +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_forward_1.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_forward_1.hpp new file mode 100644 index 000000000..e5d7d1889 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_forward_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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +namespace sm_dancebot_artgallery_ue +{ +namespace s_pattern_states +{ +// STATE DECLARATION +struct StiSPatternForward1 : public smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSPatternRotate2>, + Transition, StiSPatternRotate1> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(SS::pitch1_lenght_meters()); + configure_orthogonal(); + } + + void runtimeConfigure() {} +}; +} // namespace s_pattern_states +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_forward_2.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_forward_2.hpp new file mode 100644 index 000000000..f011dd8df --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_forward_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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +namespace sm_dancebot_artgallery_ue +{ +namespace s_pattern_states +{ +// STATE DECLARATION +struct StiSPatternForward2 : public smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSPatternRotate3>, + Transition, StiSPatternRotate2> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(SS::pitch2_lenght_meters()); + configure_orthogonal(); + } + + void runtimeConfigure() {} +}; +} // namespace s_pattern_states +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_forward_3.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_forward_3.hpp new file mode 100644 index 000000000..3075f9bdb --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_forward_3.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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +namespace sm_dancebot_artgallery_ue +{ +namespace s_pattern_states +{ +// STATE DECLARATION +struct StiSPatternForward3 : public smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSPatternRotate4>, + Transition, StiSPatternRotate3> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(SS::pitch1_lenght_meters()); + configure_orthogonal(); + } +}; +} // namespace s_pattern_states +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_forward_4.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_forward_4.hpp new file mode 100644 index 000000000..d6736d5a9 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_forward_4.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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +namespace sm_dancebot_artgallery_ue +{ +namespace s_pattern_states +{ +// STATE DECLARATION +struct StiSPatternForward4 : public smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSPatternLoopStart>, + Transition, StiSPatternRotate4> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() + { + //auto &superstate = this->context(); + + this->configure(SS::pitch2_lenght_meters()); + this->configure(); + } +}; +} // namespace s_pattern_states +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_loop_start.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_loop_start.hpp new file mode 100644 index 000000000..69e373bee --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_loop_start.hpp @@ -0,0 +1,60 @@ +// 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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_artgallery_ue +{ +namespace s_pattern_states +{ +template +struct B : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + //typedef typename TDerived::reactions reactions; +}; + +// STATE DECLARATION +struct StiSPatternLoopStart : public B +{ + using B::B; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSPatternRotate1, CONTINUELOOP> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} + + bool loopCondition() + { + auto & superstate = this->context(); + return superstate.iteration_count++ < superstate.total_iterations(); + } + + void onEntry() { checkWhileLoopConditionAndThrowEvent(&StiSPatternLoopStart::loopCondition); } +}; + +} // namespace s_pattern_states +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_1.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_1.hpp new file mode 100644 index 000000000..ec045e1a4 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_1.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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +namespace sm_dancebot_artgallery_ue +{ +namespace s_pattern_states +{ +// STATE DECLARATION +struct StiSPatternRotate1 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSPatternForward1>, + Transition, StiSPatternRotate1> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() + { + auto & superstate = this->context(); + + float offset = 0; + if (superstate.direction() == TDirection::RIGHT) + { + // - offset because we are looking to the north and we have to turn clockwise + this->configure(0 - offset); + } + else + { + // - offset because we are looking to the south and we have to turn counter-clockwise + this->configure(180 + offset); + } + } +}; +} // namespace s_pattern_states +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_2.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_2.hpp new file mode 100644 index 000000000..b2954e212 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_2.hpp @@ -0,0 +1,61 @@ +// 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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_artgallery_ue +{ +namespace s_pattern_states +{ +// STATE DECLARATION +struct StiSPatternRotate2 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSPatternForward2>, + Transition, StiSPatternForward2> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + float offset = 0; + float angle = 0; + if (SS::direction() == TDirection::LEFT) + angle = 90 + offset; + else + angle = -90 - offset; + + configure_orthogonal(angle); + configure_orthogonal(); + } + + void runtimeConfigure() + { + auto & superstate = this->context(); + RCLCPP_INFO( + getLogger(), "[StiSPatternRotate] SpatternRotate rotate: SS current iteration: %d/%d", + superstate.iteration_count, SS::total_iterations()); + } +}; +} // namespace s_pattern_states +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_3.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_3.hpp new file mode 100644 index 000000000..70680cf33 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_3.hpp @@ -0,0 +1,72 @@ +// 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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_artgallery_ue +{ +namespace s_pattern_states +{ +// STATE DECLARATION +struct StiSPatternRotate3 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSPatternForward3>, + Transition, StiSPatternRotate3> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() + { + auto & superstate = this->context(); + RCLCPP_INFO( + getLogger(), "[StiSPatternRotate] SpatternRotate rotate: SS current iteration: %d/%d", + superstate.iteration_count, SS::total_iterations()); + + float offset = 0; + // float angle = 0; + // if (superstate.direction() == TDirection::LEFT) + // angle = -90 - offset; + // else + // angle = +90 + offset; + + // this->configure(angle); + + if (superstate.direction() == TDirection::RIGHT) + { + // - offset because we are looking to the north and we have to turn clockwise + this->configure(0 - offset); + } + else + { + // - offset because we are looking to the south and we have to turn counter-clockwise + this->configure(180 + offset); + } + + this->configure(); + } +}; +} // namespace s_pattern_states +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_4.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_4.hpp new file mode 100644 index 000000000..e447f75c7 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_4.hpp @@ -0,0 +1,60 @@ +// 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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_artgallery_ue +{ +namespace s_pattern_states +{ +// STATE DECLARATION +struct StiSPatternRotate4 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSPatternForward4>, + Transition, StiSPatternRotate4> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() + { + auto & superstate = this->context(); + RCLCPP_INFO( + getLogger(), "[SsrSPatternRotate] SpatternRotate rotate: SS current iteration: %d/%d", + superstate.iteration_count, SS::total_iterations()); + + float offset = 0; + float angle = 0; + if (superstate.direction() == TDirection::LEFT) + angle = -90 - offset; + else + angle = 90 + offset; + + this->configure(angle); + this->configure(); + } +}; +} // namespace s_pattern_states +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_acquire_sensors.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_acquire_sensors.hpp new file mode 100644 index 000000000..4e0c9a4c2 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_acquire_sensors.hpp @@ -0,0 +1,73 @@ +// 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 + +namespace sm_dancebot_artgallery_ue +{ +using namespace smacc2::default_events; +using smacc2::client_behaviors::CbSleepFor; +using namespace std::chrono_literals; + +// STATE DECLARATION +struct StAcquireSensors : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct ON_SENSORS_AVAILABLE : SUCCESS{}; + struct SrAcquireSensors; + + // TRANSITION TABLE + typedef mpl::list< + + // + Transition, StInitialRoadWaypointsX, ON_SENSORS_AVAILABLE>, + Transition + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + // configure_orthogonal("Hello World!"); + // configure_orthogonal(); + // configure_orthogonal(Service3Command::SERVICE3_ON); + // configure_orthogonal(); + // configure_orthogonal(); + configure_orthogonal("waypoints_art_gallery_1", "sm_dancebot_artgallery_ue"); + configure_orthogonal(5s); + + + + + // Create State Reactor + auto srAllSensorsReady = static_createStateReactor< + SrAllEventsGo, smacc2::state_reactors::EvAllGo, + mpl::list< + // EvTopicMessage, + // EvCbSuccess, + EvCbSuccess + >>(); + } +}; +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_back_on_road_waypoints_x.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_back_on_road_waypoints_x.hpp new file mode 100644 index 000000000..0130dfd1c --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_back_on_road_waypoints_x.hpp @@ -0,0 +1,68 @@ +// 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 + +namespace sm_dancebot_artgallery_ue +{ + +// STATE DECLARATION +struct StBackOnRoadWaypointsX : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // CUSTOM TRANSITION TAGS + struct TRANSITION_1 : SUCCESS{}; + struct TRANSITION_2 : SUCCESS{}; + struct TRANSITION_3 : SUCCESS{}; + struct TRANSITION_4 : SUCCESS{}; + struct TRANSITION_5 : SUCCESS{}; + struct TRANSITION_6 : SUCCESS{}; + + // TRANSITION TABLE + typedef mpl::list< + Transition, + Transition, StBackOnRoadWaypointsX, TRANSITION_1> + // Transition, StNavigateToWaypointsX, TRANSITION_2>, + // Transition, SS1::SsRadialPattern1, TRANSITION_3>, + // Transition, SS2::SsRadialPattern2, TRANSITION_4> + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + // configure_orthogonal(); + configure_orthogonal("waypoints_plan_back_on_road", "sm_dancebot_artgallery_ue"); + configure_orthogonal(); + } + + void onEntry() + { + } + + void runtimeConfigure() {} + + void onExit(ABORT) + { + } +}; +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_final_state.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_final_state.hpp new file mode 100644 index 000000000..c856d7fea --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_final_state.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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#pragma once + +#include + +namespace sm_dancebot_artgallery_ue +{ +using namespace smacc2::default_events; +using smacc2::client_behaviors::CbSleepFor; +using namespace std::chrono_literals; + +// STATE DECLARATION +struct StFinalState : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(); + } + + void runtimeConfigure() + { + + } +}; +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_inital_road_waypoints_x.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_inital_road_waypoints_x.hpp new file mode 100644 index 000000000..186b80df6 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_inital_road_waypoints_x.hpp @@ -0,0 +1,64 @@ +// 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 + +namespace sm_dancebot_artgallery_ue +{ + +// STATE DECLARATION +struct StInitialRoadWaypointsX : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // CUSTOM TRANSITION TAGS + struct TRANSITION_1 : SUCCESS{}; + struct TRANSITION_2 : SUCCESS{}; + struct TRANSITION_3 : SUCCESS{}; + struct TRANSITION_4 : SUCCESS{}; + struct TRANSITION_5 : SUCCESS{}; + struct TRANSITION_6 : SUCCESS{}; + + // TRANSITION TABLE + typedef mpl::list< + Transition, + //Transition, StTurnAround, TRANSITION_1>, + Transition, StInitialRoadWaypointsX, TRANSITION_2> + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(); + } + + void onEntry() + { + } + + void runtimeConfigure() {} + + void onExit(ABORT) + { + } +}; +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_navigate_field_waypoints_x.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_navigate_field_waypoints_x.hpp new file mode 100644 index 000000000..b0e002970 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_navigate_field_waypoints_x.hpp @@ -0,0 +1,63 @@ +// 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 + +namespace sm_dancebot_artgallery_ue +{ + +// STATE DECLARATION +struct StNavigateFieldWaypointsX : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // CUSTOM TRANSITION TAGS + struct TRANSITION_1 : SUCCESS{}; + struct TRANSITION_2 : SUCCESS{}; + struct TRANSITION_3 : SUCCESS{}; + struct TRANSITION_4 : SUCCESS{}; + struct TRANSITION_5 : SUCCESS{}; + struct TRANSITION_6 : SUCCESS{}; + + // TRANSITION TABLE + typedef mpl::list< + Transition, StNavigateFieldWaypointsX, TRANSITION_1> + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + // configure_orthogonal(); + configure_orthogonal(); + } + + void onEntry() + { + } + + void runtimeConfigure() {} + + void onExit(ABORT) + { + } +}; +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_turn_around.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_turn_around.hpp new file mode 100644 index 000000000..a0606975e --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_turn_around.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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#pragma once + +#include + +namespace sm_dancebot_artgallery_ue +{ +using namespace smacc2::default_events; +using smacc2::client_behaviors::CbSleepFor; +using namespace std::chrono_literals; + +// STATE DECLARATION +struct StTurnAround : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + Transition, StBackOnRoadWaypointsX, SUCCESS> + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(M_PI, 0.8); + } + + void runtimeConfigure() + { + auto spinningBehavior = this->getOrthogonal()->getClientBehavior(); + spinningBehavior->yaw_goal_tolerance_rads_ = 0.2; + + } +}; +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_f_pattern_1.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_f_pattern_1.hpp new file mode 100644 index 000000000..a4c0316a8 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_f_pattern_1.hpp @@ -0,0 +1,91 @@ +// 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 + * + ******************************************************************************************************************/ + +#include + +namespace sm_dancebot_artgallery_ue +{ +namespace f_pattern_states +{ +enum class TDirection +{ + LEFT, + RIGHT +}; + +// FORWARD DECLARATIONS OF INNER STATES +template class StiFPatternRotate1; +template class StiFPatternForward1; +template class StiFPatternReturn1; +template class StiFPatternRotate2; +template class StiFPatternForward2; +template class StiFPatternStartLoop; + +} // namespace f_pattern_states +} // namespace sm_dancebot_artgallery_ue +namespace sm_dancebot_artgallery_ue +{ +namespace SS4 +{ +using namespace f_pattern_states; + +// STATE DECLARATION +struct SsFPattern1 +: smacc2::SmaccState> +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + // Transition>, StNavigateReverse4, ENDLOOP> //, + + >reactions; + + // STATE VARIABLES + // superstate parameters + static constexpr float ray_lenght_meters() { return 3.25; } + static constexpr float pitch_lenght_meters() { return 0.75; } + static constexpr int total_iterations() { return 10; } + static constexpr TDirection direction() { return TDirection::RIGHT; } + + // superstate state variables + int iteration_count; + + // STATE FUNCTIONS + static void staticConfigure() + { + } + + void runtimeConfigure() { iteration_count = 0; } +}; // namespace SS4 + +// FORWARD DECLARATION FOR THE SUPERSTATE + +} // namespace SS4 +} // namespace sm_dancebot_artgallery_ue + +#include +#include +#include +#include +#include +#include diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_radial_pattern_1.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_radial_pattern_1.hpp new file mode 100644 index 000000000..3607aa68d --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_radial_pattern_1.hpp @@ -0,0 +1,77 @@ +// 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 + * + ******************************************************************************************************************/ + +#include + +namespace sm_dancebot_artgallery_ue +{ +namespace SS1 +{ +namespace sm_dancebot_artgallery_ue +{ +namespace radial_motion_states +{ +//FORWARD DECLARATION OF INNER STATES +class StiRadialRotate; +class StiRadialReturn; +class StiRadialEndPoint; +class StiRadialLoopStart; + +} // namespace radial_motion_states +} // namespace sm_dancebot_artgallery_ue +using namespace sm_dancebot_artgallery_ue::radial_motion_states; + +// STATE DECLARATION +struct SsRadialPattern1 +: smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + // Transition, StRotateDegrees1, ENDLOOP> + // Transition, StNavigateReverse1, ENDLOOP> + + >reactions; + + static constexpr int total_iterations() { return 16; } + static constexpr float ray_angle_increment_degree() { return 360.0 / total_iterations(); } + static constexpr float ray_length_meters() { return 1.0; } + + int iteration_count = 0; + + // STATE FUNCTIONS + static void staticConfigure() + { + } + + void runtimeConfigure() {} +}; + +// FORWARD DECLARATION FOR THE SUPERSTATE +using SS = SsRadialPattern1; +#include +#include +#include +#include +} // namespace SS1 +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_radial_pattern_2.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_radial_pattern_2.hpp new file mode 100644 index 000000000..972649212 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_radial_pattern_2.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 + * + ******************************************************************************************************************/ + +#include + +namespace sm_dancebot_artgallery_ue +{ +namespace SS2 +{ +namespace sm_dancebot_artgallery_ue +{ +namespace radial_motion_states +{ +// FORWARD DECLARATIONS OF INNER STATES +class StiRadialRotate; +class StiRadialReturn; +class StiRadialEndPoint; +class StiRadialLoopStart; +} // namespace radial_motion_states +} // namespace sm_dancebot_artgallery_ue +using namespace sm_dancebot_artgallery_ue::radial_motion_states; + +// STATE DECLARATION +struct SsRadialPattern2 +: smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + // Transition, StNavigateReverse1, ENDLOOP> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + } + + void runtimeConfigure() {} + + int iteration_count = 0; + static constexpr int total_iterations() { return 16; } + static constexpr float ray_angle_increment_degree() { return 360.0 / total_iterations(); } + static constexpr float ray_length_meters() { return 5; } + + void onExit() + { + rclcpp::sleep_for(5s); + RCLCPP_INFO(getLogger(), "[SsRadialPattern1] waiting 5 seconds"); + } +}; + +// FORWARD DECLARATION FOR THE SUPERSTATE +using SS = SsRadialPattern2; +#include +#include +#include +#include +} // namespace SS2 +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_radial_pattern_3.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_radial_pattern_3.hpp new file mode 100644 index 000000000..5ef0b9888 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_radial_pattern_3.hpp @@ -0,0 +1,75 @@ +// 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 + * + ******************************************************************************************************************/ + +#include + +namespace sm_dancebot_artgallery_ue +{ +namespace SS3 +{ +namespace sm_dancebot_artgallery_ue +{ +namespace radial_motion_states +{ +// FORWARD DECLARATION OF INNER STATES +class StiRadialRotate; +class StiRadialReturn; +class StiRadialEndPoint; +class StiRadialLoopStart; +} // namespace radial_motion_states +} // namespace sm_dancebot_artgallery_ue +using namespace sm_dancebot_artgallery_ue::radial_motion_states; + +// STATE DECLARATION +struct SsRadialPattern3 +: smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + // Transition, StRotateDegrees4, ENDLOOP> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + } + + int iteration_count; + + static constexpr int total_iterations() { return 16; } + static constexpr float ray_angle_increment_degree() { return 360.0 / total_iterations(); } + static constexpr float ray_length_meters() { return 4; } + + void runtimeConfigure() { iteration_count = 0; } +}; + +// FORWARD DECLARATION FOR THE SUPERSTATE +using SS = SsRadialPattern3; +#include +#include +#include +#include +} // namespace SS3 +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_s_pattern_1.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_s_pattern_1.hpp new file mode 100644 index 000000000..84e588c7e --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_s_pattern_1.hpp @@ -0,0 +1,92 @@ +// 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 + * + ******************************************************************************************************************/ + +#include + +namespace sm_dancebot_artgallery_ue +{ +namespace SS5 +{ +namespace sm_dancebot_artgallery_ue +{ +namespace s_pattern_states +{ +// FORWARD DECLARATIONS OF INNER STATES +class StiSPatternRotate1; +class StiSPatternForward1; +class StiSPatternRotate2; +class StiSPatternForward2; +class StiSPatternRotate3; +class StiSPatternForward3; +class StiSPatternRotate4; +class StiSPatternForward4; +class StiSPatternLoopStart; +} // namespace s_pattern_states +} // namespace sm_dancebot_artgallery_ue + +enum class TDirection +{ + LEFT, + RIGHT +}; + +using namespace sm_dancebot_artgallery_ue::s_pattern_states; + +// STATE DECLARATION +struct SsSPattern1 : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + // Transition, StNavigateReverse3, ENDLOOP> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + } + + static constexpr float pitch1_lenght_meters() { return 0.75; } + static constexpr float pitch2_lenght_meters() { return 1.45; } + static constexpr int total_iterations() { return 9; } + static constexpr TDirection direction() { return TDirection::RIGHT; } + + int iteration_count; + + void runtimeConfigure() { this->iteration_count = 0; } +}; + +// FORWARD DECLARATION FOR THE SUPERSTATE +using SS = SsSPattern1; +#include +#include +#include +#include +#include +#include +#include +#include +#include +} // namespace SS5 +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/bringup_launch.py b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/bringup_launch.py new file mode 100644 index 000000000..a9804d127 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/bringup_launch.py @@ -0,0 +1,137 @@ +# 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 ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + GroupAction, + IncludeLaunchDescription, + SetEnvironmentVariable, +) +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import PushRosNamespace + + +def generate_launch_description(): + # Get the launch directory + sm_dance_bot_dir = get_package_share_directory("sm_dancebot_artgallery_ue") + launch_dir = os.path.join(sm_dance_bot_dir, "launch") + + # Create the launch configuration variables + namespace = LaunchConfiguration("namespace") + use_namespace = LaunchConfiguration("use_namespace") + slam = LaunchConfiguration("slam") + # map_yaml_file = LaunchConfiguration("map") + use_sim_time = LaunchConfiguration("use_sim_time") + params_file = LaunchConfiguration("params_file") + default_nav_to_pose_bt_xml = LaunchConfiguration("default_nav_to_pose_bt_xml") + autostart = LaunchConfiguration("autostart") + + stdout_linebuf_envvar = SetEnvironmentVariable("RCUTILS_LOGGING_BUFFERED_STREAM", "1") + + declare_namespace_cmd = DeclareLaunchArgument( + "namespace", default_value="", description="Top-level namespace" + ) + + declare_use_namespace_cmd = DeclareLaunchArgument( + "use_namespace", + default_value="false", + description="Whether to apply a namespace to the navigation stack", + ) + + declare_slam_cmd = DeclareLaunchArgument( + "slam", default_value="true", description="Whether run a SLAM" + ) + + declare_map_yaml_cmd = DeclareLaunchArgument( + "map", description="Full path to map yaml file to load" + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + "use_sim_time", default_value="false", description="Use simulation clock if true" + ) + + declare_params_file_cmd = DeclareLaunchArgument( + "params_file", + default_value=os.path.join(sm_dance_bot_dir, "params", "nav2z_client", "nav2_params.yaml"), + description="Full path to the ROS2 parameters file to use for all launched nodes", + ) + + declare_bt_xml_cmd = DeclareLaunchArgument( + "default_nav_to_pose_bt_xml", + default_value=os.path.join( + sm_dance_bot_dir, "params", "nav2z_client", "navigation_tree.xml" + ), + description="Full path to the behavior tree xml file to use", + ) + + declare_autostart_cmd = DeclareLaunchArgument( + "autostart", default_value="true", description="Automatically startup the nav2 stack" + ) + + # Specify the actions + bringup_cmd_group = GroupAction( + [ + PushRosNamespace(condition=IfCondition(use_namespace), namespace=namespace), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(launch_dir, "slam_launch.py")), + condition=IfCondition(slam), + launch_arguments={ + "namespace": namespace, + "use_sim_time": use_sim_time, + "autostart": autostart, + "params_file": params_file, + }.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(launch_dir, "navigation_launch.py")), + launch_arguments={ + "namespace": namespace, + "use_sim_time": use_sim_time, + "autostart": autostart, + "params_file": params_file, + "default_nav_to_pose_bt_xml": default_nav_to_pose_bt_xml, + "use_lifecycle_mgr": "false", + "map_subscribe_transient_local": "true", + }.items(), + ), + ] + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Set environment variables + ld.add_action(stdout_linebuf_envvar) + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_namespace_cmd) + ld.add_action(declare_slam_cmd) + ld.add_action(declare_map_yaml_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_autostart_cmd) + ld.add_action(declare_bt_xml_cmd) + + # Add the actions to launch all of the navigation nodes + ld.add_action(bringup_cmd_group) + + return ld diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/husky_gazebo.launch.yaml b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/husky_gazebo.launch.yaml new file mode 100644 index 000000000..66bb498b0 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/husky_gazebo.launch.yaml @@ -0,0 +1,3 @@ +launch: + - include: + file: "$(find-pkg-share husky_gazebo)/launch/gazebo_launch.py" diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/navigation_launch.py b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/navigation_launch.py new file mode 100644 index 000000000..f14edd17a --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/navigation_launch.py @@ -0,0 +1,169 @@ +# 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 ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + # Get the launch directory + sm_dance_bot_dir = get_package_share_directory("sm_dancebot_artgallery_ue") + + namespace = LaunchConfiguration("namespace") + use_sim_time = LaunchConfiguration("use_sim_time") + autostart = LaunchConfiguration("autostart") + params_file = LaunchConfiguration("params_file") + default_nav_to_pose_bt_xml = LaunchConfiguration("default_nav_to_pose_bt_xml") + map_subscribe_transient_local = LaunchConfiguration("map_subscribe_transient_local") + + lifecycle_nodes = ["controller_server", "planner_server", "behavior_server", "bt_navigator"] + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + # TODO(orduno) Substitute with `PushNodeRemapping` + # https://github.com/ros2/launch_ros/issues/56 + remappings = [("/tf", "tf"), ("/tf_static", "tf_static")] + + # Create our own temporary YAML files that include substitutions + param_substitutions = { + "use_sim_time": use_sim_time, + # 'default_nav_to_pose_bt_xml': default_nav_to_pose_bt_xml, + "default_nav_to_pose_bt_xml": os.path.join( + sm_dance_bot_dir, "params", "nav2z_client", "navigation_tree.xml" + ), + "autostart": autostart, + "map_subscribe_transient_local": map_subscribe_transient_local, + } + + configured_params = RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites=param_substitutions, + convert_types=True, + ) + + xtermprefix = "xterm -xrm 'XTerm*scrollBar: true' -xrm 'xterm*rightScrollBar: true' -hold -geometry 1000x600 -sl 10000 -e" + + print("+********************************") + print(str(param_substitutions)) + print(str(default_nav_to_pose_bt_xml)) + + return LaunchDescription( + [ + # Set env var to print messages to stdout immediately + SetEnvironmentVariable("RCUTILS_LOGGING_BUFFERED_STREAM", "1"), + DeclareLaunchArgument( + "namespace", default_value="", description="Top-level namespace" + ), + DeclareLaunchArgument( + "use_sim_time", + default_value="false", + description="Use simulation clock if true", + ), + DeclareLaunchArgument( + "autostart", + default_value="true", + description="Automatically startup the nav2 stack", + ), + DeclareLaunchArgument( + "params_file", + default_value=os.path.join( + sm_dance_bot_dir, "params", "nav2z_client", "nav2_params.yaml" + ), + description="Full path to the ROS2 parameters file to use", + ), + DeclareLaunchArgument( + "default_nav_to_pose_bt_xml", + # default_value=os.path.join(get_package_share_directory('nav2_bt_navigator'),'behavior_trees', 'navigate_w_replanning_and_recovery.xml'), + default_value=os.path.join( + sm_dance_bot_dir, "params", "nav2z_client", "navigation_tree.xml" + ), + description="Full path to the behavior tree xml file to use", + ), + DeclareLaunchArgument( + "map_subscribe_transient_local", + default_value="false", + description="Whether to set the map subscriber QoS to transient local", + ), + Node( + package="nav2_controller", + executable="controller_server", + output="screen", + prefix=xtermprefix, + parameters=[configured_params], + remappings=remappings, + arguments=["--ros-args", "--log-level", "INFO"], + ), + Node( + package="nav2_planner", + executable="planner_server", + name="planner_server", + output="screen", + prefix=xtermprefix, + parameters=[configured_params], + remappings=remappings, + arguments=["--ros-args", "--log-level", "INFO"], + ), + Node( + package="nav2_behaviors", + executable="behavior_server", + name="behavior_server", + output="screen", + prefix=xtermprefix, + parameters=[configured_params], + remappings=remappings, + arguments=["--ros-args", "--log-level", "INFO"], + ), + Node( + package="nav2_bt_navigator", + executable="bt_navigator", + name="bt_navigator", + output="screen", + prefix=xtermprefix, + parameters=[configured_params], + remappings=remappings, + arguments=["--ros-args", "--log-level", "INFO"], + ), + Node( + package="nav2_waypoint_follower", + executable="waypoint_follower", + name="waypoint_follower", + output="screen", + parameters=[configured_params], + remappings=remappings, + ), + Node( + package="nav2_lifecycle_manager", + executable="lifecycle_manager", + name="lifecycle_manager_navigation", + output="screen", + prefix=xtermprefix, + parameters=[ + {"use_sim_time": use_sim_time}, + {"autostart": autostart}, + {"node_names": lifecycle_nodes}, + ], + arguments=["--ros-args", "--log-level", "INFO"], + ), + ] + ) diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/online_sync_launch.py b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/online_sync_launch.py new file mode 100644 index 000000000..38c20d044 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/online_sync_launch.py @@ -0,0 +1,61 @@ +# Copyright (c) 2023 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 + + +import os + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + use_sim_time = LaunchConfiguration("use_sim_time") + slam_params_file = LaunchConfiguration("slam_params_file") + xtermprefix = "xterm -xrm 'XTerm*scrollBar: true' -xrm 'xterm*rightScrollBar: true' -hold -geometry 1000x600 -sl 10000 -e" + + declare_use_sim_time_argument = DeclareLaunchArgument( + "use_sim_time", default_value="true", description="Use simulation/Gazebo clock" + ) + declare_slam_params_file_cmd = DeclareLaunchArgument( + "slam_params_file", + default_value=os.path.join( + # get_package_share_directory("slam_toolbox"), "config", "mapper_params_online_sync.yaml" + get_package_share_directory("sm_dancebot_artgallery_ue"), + "params", + "mapper_params_online_sync.yaml", + ), + description="Full path to the ROS2 parameters file to use for the slam_toolbox node", + ) + + start_sync_slam_toolbox_node = Node( + parameters=[slam_params_file, {"use_sim_time": use_sim_time}], + package="slam_toolbox", + executable="sync_slam_toolbox_node", + name="slam_toolbox", + output="screen", + prefix=xtermprefix, + ) + + ld = LaunchDescription() + + ld.add_action(declare_use_sim_time_argument) + ld.add_action(declare_slam_params_file_cmd) + ld.add_action(start_sync_slam_toolbox_node) + + return ld diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/rviz_launch.py b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/rviz_launch.py new file mode 100644 index 000000000..056b5f8f0 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/rviz_launch.py @@ -0,0 +1,122 @@ +# 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 ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, EmitEvent, RegisterEventHandler +from launch.conditions import IfCondition, UnlessCondition +from launch.event_handlers import OnProcessExit +from launch.events import Shutdown +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from nav2_common.launch import ReplaceString + + +def generate_launch_description(): + # Get the launch directory + bringup_dir = get_package_share_directory("nav2_bringup") + + # Create the launch configuration variables + namespace = LaunchConfiguration("namespace") + use_namespace = LaunchConfiguration("use_namespace") + rviz_config_file = LaunchConfiguration("rviz_config") + + # Declare the launch arguments + declare_namespace_cmd = DeclareLaunchArgument( + "namespace", + default_value="navigation", + description=( + "Top-level namespace. The value will be used to replace the " + " keyword on the rviz config file." + ), + ) + + declare_use_namespace_cmd = DeclareLaunchArgument( + "use_namespace", + default_value="false", + description="Whether to apply a namespace to the navigation stack", + ) + + declare_rviz_config_file_cmd = DeclareLaunchArgument( + "rviz_config", + default_value=os.path.join(bringup_dir, "rviz", "nav2_default_view.rviz"), + description="Full path to the RVIZ config file to use", + ) + + # Launch rviz + start_rviz_cmd = Node( + condition=UnlessCondition(use_namespace), + package="rviz2", + executable="rviz2", + name="rviz2", + arguments=["-d", rviz_config_file], + output="screen", + ) + + namespaced_rviz_config_file = ReplaceString( + source_file=rviz_config_file, replacements={"": ("/", namespace)} + ) + + start_namespaced_rviz_cmd = Node( + condition=IfCondition(use_namespace), + package="rviz2", + executable="rviz2", + name="rviz2", + namespace=namespace, + arguments=["-d", namespaced_rviz_config_file], + output="screen", + remappings=[ + ("/tf", "tf"), + ("/tf_static", "tf_static"), + ("/goal_pose", "goal_pose"), + ("/clicked_point", "clicked_point"), + ("/initialpose", "initialpose"), + ], + ) + + exit_event_handler = RegisterEventHandler( + condition=UnlessCondition(use_namespace), + event_handler=OnProcessExit( + target_action=start_rviz_cmd, on_exit=EmitEvent(event=Shutdown(reason="rviz exited")) + ), + ) + + exit_event_handler_namespaced = RegisterEventHandler( + condition=IfCondition(use_namespace), + event_handler=OnProcessExit( + target_action=start_namespaced_rviz_cmd, + on_exit=EmitEvent(event=Shutdown(reason="rviz exited")), + ), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_namespace_cmd) + ld.add_action(declare_rviz_config_file_cmd) + + # Add any conditioned actions + ld.add_action(start_rviz_cmd) + ld.add_action(start_namespaced_rviz_cmd) + + # Add other nodes and processes we need + ld.add_action(exit_event_handler) + ld.add_action(exit_event_handler_namespaced) + + return ld diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/slam_launch.py b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/slam_launch.py new file mode 100644 index 000000000..b26de6624 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/slam_launch.py @@ -0,0 +1,128 @@ +# Copyright (c) 2020 Samsung Research Russia +# +# 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 ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + +# from launch_ros.actions import Node +# from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + # Input parameters declaration + # namespace = LaunchConfiguration("namespace") + # params_file = LaunchConfiguration("params_file") + use_sim_time = LaunchConfiguration("use_sim_time") + # autostart = LaunchConfiguration("autostart") + sm_dance_bot_dir = get_package_share_directory("sm_dancebot_artgallery_ue") + + # Variables + # lifecycle_nodes = ["slam_toolbox"] + + # Getting directories and launch-files + # bringup_dir = get_package_share_directory("nav2_bringup") + # slam_toolbox_dir = get_package_share_directory("slam_toolbox") + # slam_launch_file = os.path.join(sm_dance_bot_dir, 'launch', 'online_sync_launch.py') + slam_launch_file = os.path.join(sm_dance_bot_dir, "launch", "online_sync_launch.py") + + # Create our own temporary YAML files that include substitutions + # param_substitutions = {"use_sim_time": use_sim_time} + + # configured_params = RewrittenYaml( + # source_file=params_file, + # root_key=namespace, + # param_rewrites=param_substitutions, + # convert_types=True, + # ) + + # Declare the launch arguments + declare_namespace_cmd = DeclareLaunchArgument( + "namespace", default_value="", description="Top-level namespace" + ) + + # declare_params_file_cmd = DeclareLaunchArgument( + # 'params_file', + # default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), + # description='Full path to the ROS2 parameters file to use for all launched nodes') + + declare_use_sim_time_cmd = DeclareLaunchArgument( + "use_sim_time", default_value="True", description="Use simulation (Gazebo) clock if true" + ) + + declare_autostart_cmd = DeclareLaunchArgument( + "autostart", default_value="True", description="Automatically startup the nav2 stack" + ) + + # Nodes launching commands + + # start_map_saver_server_cmd = Node( + # package='nav2_map_server', + # executable='map_saver_server', + # output='screen', + # parameters=[configured_params]) + + # xtermprefix = "xterm -xrm 'XTerm*scrollBar: true' -xrm 'xterm*rightScrollBar: true' " \ + # "-hold -geometry 1000x600 -sl 10000 -e" + + # start_lifecycle_manager_cmd = Node( + # package="nav2_lifecycle_manager", + # executable="lifecycle_manager", + # name="lifecycle_manager_slam", + # output="screen", + # parameters=[ + # {"use_sim_time": use_sim_time}, + # {"autostart": autostart}, + # {"node_names": lifecycle_nodes}, + # ], + # prefix=xtermprefix, + # ) + + # If the provided param file doesn't have slam_toolbox params, we must remove the 'params_file' + # LaunchConfiguration, or it will be passed automatically to slam_toolbox and will not load + # the default file + + start_slam_toolbox_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource(slam_launch_file), + launch_arguments={"use_sim_time": use_sim_time}.items(), + ) + + # Pop (or load) previous LaunchConfiguration, resetting the state of params_file + # pop_launch_config = PopLaunchConfigurations( + # condition=UnlessCondition(has_slam_toolbox_params)) + + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + # ld.add_action(declare_params_file_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_autostart_cmd) + + # Running Map Saver Server + # ld.add_action(start_map_saver_server_cmd) + # ld.add_action(start_lifecycle_manager_cmd) + + # Running SLAM Toolbox + # ld.add_action(push_launch_config) + # ld.add_action(remove_params_file) + ld.add_action(start_slam_toolbox_cmd) + # ld.add_action(pop_launch_config) + + return ld diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/sm_dancebot_artgallery_ue_launch.py b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/sm_dancebot_artgallery_ue_launch.py new file mode 100644 index 000000000..3717a7175 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/sm_dancebot_artgallery_ue_launch.py @@ -0,0 +1,267 @@ +# 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. + +"""This is all-in-one launch script intended for use by nav2 developers.""" + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + + xtermprefix = "xterm -xrm 'XTerm*scrollBar: true' -xrm 'xterm*rightScrollBar: true' -hold -geometry 1000x600 -sl 10000 -e" + + # Get the launch directory + sm_dance_bot_dir = get_package_share_directory("sm_dancebot_artgallery_ue") + sm_dance_bot_launch_dir = os.path.join(sm_dance_bot_dir, "launch") + + # Create the launch configuration variables + slam = LaunchConfiguration("slam") + namespace = LaunchConfiguration("namespace") + use_namespace = LaunchConfiguration("use_namespace") + map_yaml_file = LaunchConfiguration("map") + use_sim_time = LaunchConfiguration("use_sim_time") + + params_file = LaunchConfiguration("params_file") + default_nav_to_pose_bt_xml = LaunchConfiguration("default_nav_to_pose_bt_xml") + autostart = LaunchConfiguration("autostart") + + # Launch configuration variables specific to simulation + rviz_config_file = LaunchConfiguration("rviz_config_file") + + use_robot_state_pub = LaunchConfiguration("use_robot_state_pub") + use_rviz = LaunchConfiguration("use_rviz") + + urdf = os.path.join(sm_dance_bot_dir, "urdf", "turtlebot3_waffle.urdf") + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + # TODO(orduno) Substitute with `PushNodeRemapping` + # https://github.com/ros2/launch_ros/issues/56 + remappings = [("/tf", "tf"), ("/tf_static", "tf_static")] + + # Declare the launch arguments + declare_namespace_cmd = DeclareLaunchArgument( + "namespace", default_value="", description="Top-level namespace" + ) + + declare_use_namespace_cmd = DeclareLaunchArgument( + "use_namespace", + default_value="false", + description="Whether to apply a namespace to the navigation stack", + ) + + declare_show_gz_lidar = DeclareLaunchArgument( + "show_gz_lidar", + default_value="true", + description="Whether to apply a namespace to the navigation stack", + ) + + declare_slam_cmd = DeclareLaunchArgument( + "slam", default_value="True", description="Whether run a SLAM" + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + "use_sim_time", default_value="true", description="Use simulation clock if true" + ) + + declare_gazebo_headless_cmd = DeclareLaunchArgument( + "headless", + default_value="false", + description="Use headless Gazebo if true", + ) + + declare_params_file_cmd = DeclareLaunchArgument( + "params_file", + default_value=os.path.join(sm_dance_bot_dir, "params", "nav2z_client", "nav2_params.yaml"), + description="Full path to the ROS2 parameters file to use for all launched nodes", + ) + + declare_bt_xml_cmd = DeclareLaunchArgument( + "default_nav_to_pose_bt_xml", + default_value=os.path.join( + sm_dance_bot_dir, "params", "nav2z_client", "navigation_tree.xml" + ), + description="Full path to the behavior tree xml file to use", + ) + + declare_autostart_cmd = DeclareLaunchArgument( + "autostart", default_value="true", description="Automatically startup the nav2 stack" + ) + + declare_rviz_config_file_cmd = DeclareLaunchArgument( + "rviz_config_file", + default_value=os.path.join(sm_dance_bot_dir, "rviz", "nav2_default_view.rviz"), + description="Full path to the RVIZ config file to use", + ) + + declare_use_robot_state_pub_cmd = DeclareLaunchArgument( + "use_robot_state_pub", + default_value="True", + description="Whether to start the robot state publisher", + ) + + declare_use_rviz_cmd = DeclareLaunchArgument( + "use_rviz", default_value="True", description="Whether to start RVIZ" + ) + + declare_map_yaml_cmd = DeclareLaunchArgument( + "map", + default_value=os.path.join(sm_dance_bot_dir, "maps", "turtlebot3_world.yaml"), + description="Full path to map file to load", + ) + + start_robot_state_publisher_cmd = Node( + condition=IfCondition(use_robot_state_pub), + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + namespace=namespace, + output="screen", + parameters=[{"use_sim_time": use_sim_time}], + remappings=remappings, + arguments=[urdf], + prefix = xtermprefix + ) + + static_transform_publisher = Node( + package="sm_dancebot_artgallery_ue", + executable="transform_publisher.py", + name="static_transform_publisherxx", + output="screen", + # arguments=["-0.064", "0", "0.122", "0", "0", "0", "base_scan", "base_link"], + prefix=xtermprefix, + parameters=[{"use_sim_time": use_sim_time}], + ) + + # static_transform_publisher_2 = Node( + # package="tf2_ros", + # executable="static_transform_publisher", + # name="static_transform_publisher", + # output="screen", + # arguments=["0", "0", "0", "0", "0", "0", "base_link", "base_footprint"], + # prefix=xtermprefix, + # ) + + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(sm_dance_bot_launch_dir, "rviz_launch.py")), + condition=IfCondition(use_rviz), + launch_arguments={ + "namespace": "", + "use_namespace": "False", + "rviz_config": rviz_config_file, + }.items(), + ) + + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(sm_dance_bot_launch_dir, "bringup_launch.py")), + launch_arguments={ + "namespace": namespace, + "use_namespace": use_namespace, + "autostart": autostart, + "params_file": params_file, + "slam": slam, + "map": map_yaml_file, + "use_sim_time": use_sim_time, + "default_nav_to_pose_bt_xml": default_nav_to_pose_bt_xml, + }.items(), + ) + + sm_dance_bot_node = Node( + package="sm_dancebot_artgallery_ue", + executable="sm_dancebot_artgallery_ue_node", + name="SmDanceBotArtGalleryUE", + output="screen", + prefix=xtermprefix, + parameters=[ + os.path.join( + get_package_share_directory("sm_dancebot_artgallery_ue"), + "params/sm_dancebot_artgallery_ue_config.yaml", + ) + ], + remappings=[ + # ("/odom", "/odometry/filtered"), + # ("/sm_dance_bot_2/odom_tracker/odom_tracker_path", "/odom_tracker_path"), + # ("/sm_dance_bot_2/odom_tracker/odom_tracker_stacked_path", "/odom_tracker_path_stacked") + ], + arguments=["--ros-args", "--log-level", "INFO"], + ) + + led_action_server_node = Node( + package="sm_dancebot_artgallery_ue", + executable="led_action_server_node", + output="screen", + prefix=xtermprefix, + ) + + temperature_action_server = Node( + package="sm_dancebot_artgallery_ue", + executable="temperature_sensor_node", + output="screen", + prefix=xtermprefix, + ) + + service3_node = Node( + package="sm_dancebot_artgallery_ue", + executable="service_node_3.py", + output="screen", + prefix=xtermprefix, + parameters=[ + {"autostart": True, "node_names": ["ss", "dfa"]}, + ], + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_namespace_cmd) + ld.add_action(declare_slam_cmd) + ld.add_action(declare_map_yaml_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_gazebo_headless_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_bt_xml_cmd) + ld.add_action(declare_autostart_cmd) + ld.add_action(declare_map_yaml_cmd) + ld.add_action(declare_show_gz_lidar) + + ld.add_action(declare_rviz_config_file_cmd) + ld.add_action(declare_use_robot_state_pub_cmd) + ld.add_action(declare_use_rviz_cmd) + + ld.add_action(sm_dance_bot_node) + # ld.add_action(service3_node) + # ld.add_action(temperature_action_server) + # ld.add_action(led_action_server_node) + ld.add_action(static_transform_publisher) + # ld.add_action(static_transform_publisher_2) + + # # Add the actions to launch all of the navigation nodes + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + + return ld diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/package.xml b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/package.xml new file mode 100644 index 000000000..4189699a2 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/package.xml @@ -0,0 +1,48 @@ + + + + sm_dancebot_artgallery_ue + 2.2.2 + The dance_bot package + Pablo Inigo Blasco + + Apache-2.0 + + ament_cmake + + nav2z_client + multirole_sensor_client + navigation2 + nav2_bringup + + robot_state_publisher + ros_publisher_client + ros_timer_client + slam_toolbox + smacc2 + std_msgs + sr_all_events_go + sr_event_countdown + sr_conditional + visualization_msgs + tf2 + ue_msgs + + backward_global_planner + backward_local_planner + forward_global_planner + forward_local_planner + nav2z_planners_common + pure_spinning_local_planner + undo_path_global_planner + + rosidl_default_generators + action_msgs + rosidl_interface_packages + rosidl_default_runtime + + + ament_cmake + + + diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/mapper_params_online_sync.yaml b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/mapper_params_online_sync.yaml new file mode 100644 index 000000000..9b56c9f21 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/mapper_params_online_sync.yaml @@ -0,0 +1,73 @@ +slam_toolbox: + ros__parameters: + + # Plugin params + solver_plugin: solver_plugins::CeresSolver + ceres_linear_solver: SPARSE_NORMAL_CHOLESKY + ceres_preconditioner: SCHUR_JACOBI + ceres_trust_strategy: LEVENBERG_MARQUARDT + ceres_dogleg_type: TRADITIONAL_DOGLEG + ceres_loss_function: None + + # ROS Parameters + odom_frame: odom + map_frame: map + base_frame: base_footprint + scan_topic: /scan + mode: mapping #localization + + # if you'd like to immediately start continuing a map at a given pose + # or at the dock, but they are mutually exclusive, if pose is given + # will use pose + #map_file_name: test_steve + #map_start_pose: [0.0, 0.0, 0.0] + #map_start_at_dock: true + + debug_logging: true + throttle_scans: 1 + transform_publish_period: 0.02 #if 0 never publishes odometry + map_update_interval: 0.05 + resolution: 0.04 + max_laser_range: 16.0 #for rastering images + minimum_time_interval: 0.3 + transform_timeout: 1.0 + tf_buffer_duration: 60. + stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps + enable_interactive_mode: true + + # General Parameters + use_scan_matching: true + use_scan_barycenter: false + minimum_travel_distance: 0.1 + minimum_travel_heading: 0.1 + scan_buffer_size: 10 + scan_buffer_maximum_scan_distance: 10.0 + link_match_minimum_response_fine: 0.1 + link_scan_maximum_distance: 1.5 + loop_search_maximum_distance: 3.0 + do_loop_closing: true + loop_match_minimum_chain_size: 10 + loop_match_maximum_variance_coarse: 3.0 + loop_match_minimum_response_coarse: 0.35 + loop_match_minimum_response_fine: 0.45 + + # Correlation Parameters - Correlation Parameters + correlation_search_space_dimension: 0.5 + correlation_search_space_resolution: 0.01 + correlation_search_space_smear_deviation: 0.1 + + # Correlation Parameters - Loop Closure Parameters + loop_search_space_dimension: 8.0 + loop_search_space_resolution: 0.05 + loop_search_space_smear_deviation: 0.03 + + # Scan Matcher Parameters + distance_variance_penalty: 0.5 + angle_variance_penalty: 1.0 + + fine_search_angle_offset: 0.00349 + coarse_search_angle_offset: 0.349 + coarse_angle_resolution: 0.0349 + minimum_angle_penalty: 0.9 + minimum_distance_penalty: 0.5 + use_response_expansion: true diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/nav2_params.yaml b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/nav2_params.yaml new file mode 100644 index 000000000..d6a5fad47 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/nav2_params.yaml @@ -0,0 +1,374 @@ +amcl: + ros__parameters: + use_sim_time: True + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "base_footprint" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "differential" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 1.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: scan + +amcl_map_client: + ros__parameters: + use_sim_time: True + +amcl_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_footprint + odom_topic: /odom + enable_groot_monitoring: True + groot_zmq_publisher_port: 1666 + groot_zmq_server_port: 1667 + default_nav_to_pose_bt_xml: RUNTIMEFILL + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_back_up_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + + +bt_navigator_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["goal_checker", "forward_goal_checker", "backward_goal_checker", "absolute_rotate_goal_checker", "undo_path_backwards_goal_checker"] + controller_plugins: ["FollowPath","BackwardLocalPlanner", "ForwardLocalPlanner", "PureSpinningLocalPlanner"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.001 + movement_time_allowance: 3000.0 + + # Goal checker parameters + goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.257 + yaw_goal_tolerance: 0.165 # almost free orientation + stateful: True + backward_goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.09 + yaw_goal_tolerance: 0.05 + stateful: True + forward_goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.09 + yaw_goal_tolerance: 0.05 + stateful: True + absolute_rotate_goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.15 + yaw_goal_tolerance: 0.01 #0.005 + stateful: True + undo_path_backwards_goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.03 # 4 cm + yaw_goal_tolerance: 0.1 + stateful: True + + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.3 + max_vel_y: 0.0 + max_vel_theta: 0.5 + min_speed_xy: 0.0 + max_speed_xy: 0.05 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.157 + yaw_goal_tolerance: 0.15 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + BackwardLocalPlanner: + plugin: "backward_local_planner::BackwardLocalPlanner" + transform_tolerance: 0.5 + k_rho: -1.4 # linear distance to the goal gain, default = -1.0 (related with carrot distance) + k_alpha: -0.1 #-0.1 + k_betta: -0.1 # -1.0 + carrot_distance: 0.3 #meters default 0.5 + carrot_angular_distance: 0.3 # no constraint, max 3.1416 + pure_spinning_straight_line_mode: true + max_linear_x_speed: 0.25 + ForwardLocalPlanner: + plugin: "cl_nav2z::forward_local_planner::ForwardLocalPlanner" + transform_tolerance: 0.5 + k_rho: 2.5 + k_alpha: -0.4 + k_betta: -1.0 + carrot_distance: 0.2 #meters + carrot_angular_distance: 0.5 # no constraint, max 3.1416 + max_linear_x_speed: 0.3 + max_angular_z_speed: 0.2 + PureSpinningLocalPlanner: + plugin: "pure_spinning_local_planner::PureSpinningLocalPlanner" + transform_tolerance: 0.5 + k_betta: 10.0 + max_angular_z_speed: 0.2 + use_shortest_angular_distance: true + + +controller_server_rclcpp_node: + ros__parameters: + use_sim_time: True + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_footprint #base_link + use_sim_time: True + rolling_window: true + transform_tolerance: 15.0 + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 15.0 + raytrace_min_range: 0.0 + obstacle_max_range: 15.5 + obstacle_min_range: 0.0 + static_layer: + map_subscribe_transient_local: True + always_send_full_costmap: True + local_costmap_client: + ros__parameters: + use_sim_time: True + local_costmap_rclcpp_node: + ros__parameters: + use_sim_time: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 2.0 + publish_frequency: 1.0 + transform_tolerance: 15.0 + width: 16 + height: 16 + global_frame: map + robot_base_frame: base_footprint + use_sim_time: True + rolling_window: true + + robot_radius: 0.22 + resolution: 0.1 + width: 8 + height: 8 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 15.0 + raytrace_min_range: 0.0 + obstacle_max_range: 15.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 1.0 + inflation_radius: 0.55 + always_send_full_costmap: True + global_costmap_client: + ros__parameters: + use_sim_time: True + global_costmap_rclcpp_node: + ros__parameters: + use_sim_time: True + +map_server: + ros__parameters: + use_sim_time: True + yaml_filename: "turtlebot3_world.yaml" + +map_saver: + ros__parameters: + use_sim_time: True + save_map_timeout: 5000 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased", "ForwardGlobalPlanner", "BackwardGlobalPlanner", "UndoPathGlobalPlanner"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 10.0 #meters + use_astar: false + allow_unknown: true + ForwardGlobalPlanner: + plugin: "forward_global_planner::ForwardGlobalPlanner" + transform_tolerance: 0.5 + BackwardGlobalPlanner: + plugin: "backward_global_planner::BackwardGlobalPlanner" + transform_tolerance: 0.5 + UndoPathGlobalPlanner: + plugin: "undo_path_global_planner::UndoPathGlobalPlanner" + transform_tolerance: 0.5 + +planner_server_rclcpp_node: + ros__parameters: + use_sim_time: True + +recoveries_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + recovery_plugins: ["spin", "backup", "wait"] + #recovery_plugins: ["spin"] + spin: + plugin: "nav2_recoveries/Spin" + backup: + plugin: "nav2_recoveries/BackUp" + wait: + plugin: "nav2_recoveries/Wait" + global_frame: odom + robot_base_frame: base_footprint + transform_timeout: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/navigation_tree.xml b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/navigation_tree.xml new file mode 100644 index 000000000..ebd9f201f --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/navigation_tree.xml @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/waypoints_art_gallery.yaml b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/waypoints_art_gallery.yaml new file mode 100644 index 000000000..7d0de55a9 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/waypoints_art_gallery.yaml @@ -0,0 +1,141 @@ +waypoints: + # POINT R1 + - position: + x: 18.10907715 + y: -52.59328613 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT R2 + - position: + x: -21.0 + y: -40.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT R3 + - position: + x: -21.0 + y: -63.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT R4 + - position: + x: -2.0 + y: -67.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT R5 + - position: + x: -10.5 + y: -63.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT R7 + - position: + x: 2.0 + y: -35.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT R8 + - position: + x: 3.0 + y: -46.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT R10 + - position: + x: -30.88127197 + y: -53.87810547 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT R11 + - position: + x: -16.21995605 + y: -35.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT R12 + - position: + x: -28.99974121 + y: -66.22507324 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT R13 + - position: + x: -3.86228577 + y: -52.00396973 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT R14 + - position: + x: -38.07478027 + y: -63.84986816 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT R15 + - position: + x: -38.07478027 + y: -48.30913086 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT R16 + - position: + x: -7.86228577 + y: -41.00396973 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client_husky/nav2_params.yaml b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client_husky/nav2_params.yaml new file mode 100644 index 000000000..08e344730 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client_husky/nav2_params.yaml @@ -0,0 +1,367 @@ +amcl: + ros__parameters: + use_sim_time: True + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "base_footprint" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "differential" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 1.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: scan + +amcl_map_client: + ros__parameters: + use_sim_time: True + +amcl_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + enable_groot_monitoring: True + groot_zmq_publisher_port: 1666 + groot_zmq_server_port: 1667 + default_nav_to_pose_bt_xml: RUNTIMEFILL + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_back_up_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + + +bt_navigator_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["goal_checker", "forward_goal_checker", "backward_goal_checker", "absolute_rotate_goal_checker", "undo_path_backwards_goal_checker"] + controller_plugins: ["FollowPath","BackwardLocalPlanner", "ForwardLocalPlanner", "PureSpinningLocalPlanner"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.001 + movement_time_allowance: 3000.0 + + # Goal checker parameters + goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.257 + yaw_goal_tolerance: 0.165 # almost free orientation + stateful: True + backward_goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.09 + yaw_goal_tolerance: 0.05 + stateful: True + forward_goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.09 + yaw_goal_tolerance: 0.05 + stateful: True + absolute_rotate_goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.15 + yaw_goal_tolerance: 0.01 #0.005 + stateful: True + undo_path_backwards_goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.03 # 4 cm + yaw_goal_tolerance: 0.1 + stateful: True + + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.3 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.05 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.157 + yaw_goal_tolerance: 0.15 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + BackwardLocalPlanner: + plugin: "backward_local_planner::BackwardLocalPlanner" + transform_tolerance: 0.5 + k_rho: -1.4 # linear distance to the goal gain, default = -1.0 (related with carrot distance) + k_alpha: -0.1 #-0.1 + k_betta: -0.1 # -1.0 + carrot_distance: 0.3 #meters default 0.5 + carrot_angular_distance: 0.3 # no constraint, max 3.1416 + pure_spinning_straight_line_mode: true + max_linear_x_speed: 0.25 + ForwardLocalPlanner: + plugin: "cl_nav2z::forward_local_planner::ForwardLocalPlanner" + transform_tolerance: 0.5 + k_rho: 2.5 + k_alpha: -0.4 + k_betta: -1.0 + carrot_distance: 0.2 #meters + carrot_angular_distance: 0.5 # no constraint, max 3.1416 + max_linear_x_speed: 0.3 + max_angular_z_speed: 0.2 + PureSpinningLocalPlanner: + plugin: "pure_spinning_local_planner::PureSpinningLocalPlanner" + transform_tolerance: 0.5 + k_betta: 10.0 + max_angular_z_speed: 0.2 + use_shortest_angular_distance: true + + +controller_server_rclcpp_node: + ros__parameters: + use_sim_time: True + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + use_sim_time: True + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 15.0 + raytrace_min_range: 0.0 + obstacle_max_range: 15.5 + obstacle_min_range: 0.0 + static_layer: + map_subscribe_transient_local: True + always_send_full_costmap: True + local_costmap_client: + ros__parameters: + use_sim_time: True + local_costmap_rclcpp_node: + ros__parameters: + use_sim_time: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + transform_tolerance: 5.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 15.0 + raytrace_min_range: 0.0 + obstacle_max_range: 15.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + global_costmap_client: + ros__parameters: + use_sim_time: True + global_costmap_rclcpp_node: + ros__parameters: + use_sim_time: True + +map_server: + ros__parameters: + use_sim_time: True + yaml_filename: "turtlebot3_world.yaml" + +map_saver: + ros__parameters: + use_sim_time: True + save_map_timeout: 5000 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased", "ForwardGlobalPlanner", "BackwardGlobalPlanner", "UndoPathGlobalPlanner"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 10.0 #meters + use_astar: false + allow_unknown: true + ForwardGlobalPlanner: + plugin: "forward_global_planner::ForwardGlobalPlanner" + transform_tolerance: 0.5 + BackwardGlobalPlanner: + plugin: "backward_global_planner::BackwardGlobalPlanner" + transform_tolerance: 0.5 + UndoPathGlobalPlanner: + plugin: "undo_path_global_planner::UndoPathGlobalPlanner" + transform_tolerance: 0.5 + +planner_server_rclcpp_node: + ros__parameters: + use_sim_time: True + +recoveries_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + recovery_plugins: ["spin", "backup", "wait"] + #recovery_plugins: ["spin"] + spin: + plugin: "nav2_recoveries/Spin" + backup: + plugin: "nav2_recoveries/BackUp" + wait: + plugin: "nav2_recoveries/Wait" + global_frame: odom + robot_base_frame: base_link + transform_timeout: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client_husky/navigation_tree.xml b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client_husky/navigation_tree.xml new file mode 100644 index 000000000..e599e5d4f --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client_husky/navigation_tree.xml @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client_husky/waypoints_plan.yaml b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client_husky/waypoints_plan.yaml new file mode 100644 index 000000000..64adbe1b0 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client_husky/waypoints_plan.yaml @@ -0,0 +1,82 @@ +waypoints: + # POINT 0 + - position: + x: 3.0 + y: -2.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 1 + - position: + x: -5.75 + y: 3.5 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 2 + - position: + x: -10.25 + y: -4.25 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 3 + - position: + x: 2.0 + y: -8.58 # north west corner of the room + #y: -12.35 # south west corner of the room + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 4 + - position: + x: -12.00 + y: 12.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 5 + - position: + x: -10.0 + y: 14.5 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 6 + - position: + x: -0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + #POINT 7 + - position: + x: 6.0 + y: 8.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client_turtlebot/nav2_params.yaml b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client_turtlebot/nav2_params.yaml new file mode 100644 index 000000000..57d8d63ad --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client_turtlebot/nav2_params.yaml @@ -0,0 +1,369 @@ +amcl: + ros__parameters: + use_sim_time: True + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "base_footprint" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "differential" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 1.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: scan + +amcl_map_client: + ros__parameters: + use_sim_time: True + +amcl_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + enable_groot_monitoring: True + groot_zmq_publisher_port: 1666 + groot_zmq_server_port: 1667 + default_nav_to_pose_bt_xml: RUNTIMEFILL + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_back_up_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + + +bt_navigator_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["goal_checker", "forward_goal_checker", "backward_goal_checker", "absolute_rotate_goal_checker", "undo_path_backwards_goal_checker"] + controller_plugins: ["FollowPath","BackwardLocalPlanner", "ForwardLocalPlanner", "PureSpinningLocalPlanner"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.001 + movement_time_allowance: 3000.0 + + # Goal checker parameters + goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.257 + yaw_goal_tolerance: 0.165 # almost free orientation + stateful: True + backward_goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.09 + yaw_goal_tolerance: 0.05 + stateful: True + forward_goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.09 + yaw_goal_tolerance: 0.05 + stateful: True + absolute_rotate_goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.15 + yaw_goal_tolerance: 0.01 #0.005 + stateful: True + undo_path_backwards_goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.03 # 4 cm + yaw_goal_tolerance: 0.1 + stateful: True + + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.3 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.3 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.157 + yaw_goal_tolerance: 0.15 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + BackwardLocalPlanner: + plugin: "backward_local_planner::BackwardLocalPlanner" + transform_tolerance: 0.5 + k_rho: -1.4 # linear distance to the goal gain, default = -1.0 (related with carrot distance) + k_alpha: -0.1 #-0.1 + k_betta: -0.1 # -1.0 + carrot_distance: 0.3 #meters default 0.5 + carrot_angular_distance: 0.3 # no constraint, max 3.1416 + pure_spinning_straight_line_mode: true + max_linear_x_speed: 0.25 + ForwardLocalPlanner: + plugin: "cl_nav2z::forward_local_planner::ForwardLocalPlanner" + transform_tolerance: 0.5 + k_rho: 2.5 + k_alpha: -0.4 + k_betta: -1.0 + carrot_distance: 0.2 #meters + carrot_angular_distance: 0.5 # no constraint, max 3.1416 + max_linear_x_speed: 0.3 + max_angular_z_speed: 0.2 + PureSpinningLocalPlanner: + plugin: "pure_spinning_local_planner::PureSpinningLocalPlanner" + transform_tolerance: 0.5 + k_betta: 10.0 + max_angular_z_speed: 0.2 + use_shortest_angular_distance: true + + +controller_server_rclcpp_node: + ros__parameters: + use_sim_time: True + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + use_sim_time: True + rolling_window: true + width: 6 + height: 6 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 15.0 + raytrace_min_range: 0.0 + obstacle_max_range: 15.5 + obstacle_min_range: 0.0 + static_layer: + map_subscribe_transient_local: True + always_send_full_costmap: True + local_costmap_client: + ros__parameters: + use_sim_time: True + local_costmap_rclcpp_node: + ros__parameters: + use_sim_time: True + +global_costmap: + global_costmap: + ros__parameters: + width: 8 + height: 8 + update_frequency: 1.0 + publish_frequency: 1.0 + transform_tolerance: 5.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 15.0 + raytrace_min_range: 0.0 + obstacle_max_range: 15.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + global_costmap_client: + ros__parameters: + use_sim_time: True + global_costmap_rclcpp_node: + ros__parameters: + use_sim_time: True + +map_server: + ros__parameters: + use_sim_time: True + yaml_filename: "turtlebot3_world.yaml" + +map_saver: + ros__parameters: + use_sim_time: True + save_map_timeout: 5000 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased", "ForwardGlobalPlanner", "BackwardGlobalPlanner", "UndoPathGlobalPlanner"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 10.0 #meters + use_astar: false + allow_unknown: true + ForwardGlobalPlanner: + plugin: "forward_global_planner::ForwardGlobalPlanner" + transform_tolerance: 0.5 + BackwardGlobalPlanner: + plugin: "backward_global_planner::BackwardGlobalPlanner" + transform_tolerance: 0.5 + UndoPathGlobalPlanner: + plugin: "undo_path_global_planner::UndoPathGlobalPlanner" + transform_tolerance: 0.5 + +planner_server_rclcpp_node: + ros__parameters: + use_sim_time: True + +recoveries_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + recovery_plugins: ["spin", "backup", "wait"] + #recovery_plugins: ["spin"] + spin: + plugin: "nav2_recoveries/Spin" + backup: + plugin: "nav2_recoveries/BackUp" + wait: + plugin: "nav2_recoveries/Wait" + global_frame: odom + robot_base_frame: base_link + transform_timeout: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client_turtlebot/navigation_tree.xml b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client_turtlebot/navigation_tree.xml new file mode 100644 index 000000000..e599e5d4f --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client_turtlebot/navigation_tree.xml @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client_turtlebot/waypoints_plan.yaml b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client_turtlebot/waypoints_plan.yaml new file mode 100644 index 000000000..64adbe1b0 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client_turtlebot/waypoints_plan.yaml @@ -0,0 +1,82 @@ +waypoints: + # POINT 0 + - position: + x: 3.0 + y: -2.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 1 + - position: + x: -5.75 + y: 3.5 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 2 + - position: + x: -10.25 + y: -4.25 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 3 + - position: + x: 2.0 + y: -8.58 # north west corner of the room + #y: -12.35 # south west corner of the room + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 4 + - position: + x: -12.00 + y: 12.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 5 + - position: + x: -10.0 + y: 14.5 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 6 + - position: + x: -0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + #POINT 7 + - position: + x: 6.0 + y: 8.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/sm_dancebot_artgallery_ue_config.yaml b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/sm_dancebot_artgallery_ue_config.yaml new file mode 100644 index 000000000..225135196 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/sm_dancebot_artgallery_ue_config.yaml @@ -0,0 +1,10 @@ +SmDanceBotArtGalleryUE: + ros__parameters: + use_sim_time: true + signal_detector_loop_freq: 20.0 + clear_angular_distance_threshold: 0.1 # 0.05 + clear_point_distance_threshold: 0.4 #0.1 + record_angular_distance_threshold: 0.005 + record_point_distance_threshold: 0.1 + + waypoints_art_gallery_1: $(pkg_share)/params/nav2z_client/waypoints_art_gallery.yaml diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/rviz/nav2_default_view.rviz b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/rviz/nav2_default_view.rviz new file mode 100644 index 000000000..e8183d2c2 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/rviz/nav2_default_view.rviz @@ -0,0 +1,805 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /TF1 + - /TF1/Frames1 + - /TF1/Tree1 + - /Global Planner1/Global Costmap1 + - /Controller1/Local Costmap1 + - /LaserScan1 + - /LaserScan1/Topic1 + Splitter Ratio: 0.5833333134651184 + Tree Height: 505 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: nav2_rviz_plugins/Navigation 2 + Name: Navigation 2 +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 + - 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: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: "" + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: false + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + base_footprint: + Value: true + base_link: + Value: true + base_scan: + Value: true + camera_depth_frame: + Value: true + camera_depth_optical_frame: + Value: true + camera_link: + Value: true + camera_rgb_frame: + Value: true + camera_rgb_optical_frame: + Value: true + caster_back_left_link: + Value: true + caster_back_right_link: + Value: true + imu_link: + Value: true + map: + Value: true + odom: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: false + Show Axes: true + Show Names: false + Tree: + map: + odom: + base_footprint: + base_link: + base_scan: + {} + camera_link: + camera_depth_frame: + camera_depth_optical_frame: + {} + camera_rgb_frame: + camera_rgb_optical_frame: + {} + caster_back_left_link: + {} + caster_back_right_link: + {} + imu_link: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: "" + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Bumper Hit + Position Transformer: "" + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Spheres + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /mobile_base/sensors/bumper_pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: true + Enabled: true + Name: Map + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Arrow Length: 0.019999999552965164 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz_default_plugins/PoseArray + Color: 0; 180; 0 + Enabled: true + Head Length: 0.07000000029802322 + Head Radius: 0.029999999329447746 + Name: Amcl Particle Swarm + Shaft Length: 0.23000000417232513 + Shaft Radius: 0.009999999776482582 + Shape: Arrow (Flat) + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /particlecloud + Value: true + - Class: rviz_common/Group + Displays: + - Alpha: 0.10000000149011612 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Global Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 0; 0 + Enabled: true + Head Diameter: 0.019999999552965164 + Head Length: 0.019999999552965164 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: Arrows + Radius: 0.029999999329447746 + Shaft Diameter: 0.004999999888241291 + Shaft Length: 0.019999999552965164 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /plan + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud + Color: 125; 125; 125 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: VoxelGrid + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Boxes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/voxel_marked_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: false + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/published_footprint + Value: false + Enabled: true + Name: Global Planner + - Class: rviz_common/Group + Displays: + - Alpha: 0.10000000149011612 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Local Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 0; 12; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Local Plan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_plan + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Trajectories + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /marker + Value: false + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: true + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/published_footprint + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: VoxelGrid + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/voxel_marked_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Controller + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: RealsenseCamera + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /intel_realsense_r200_depth/image_raw + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: RealsenseDepthImage + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /intel_realsense_r200_depth/points + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: false + Name: Realsense + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.10000000149011612 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: CpOdomTracker + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: Axes + Radius: 0.009999999776482582 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /odom_tracker_path + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: BackwardsGlobalPlan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /backward_planner/global_plan + Value: true + - Angle Tolerance: 0.10000000149011612 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: false + Enabled: true + Keep: 10000 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0 + Head Radius: 0 + Shaft Length: 0.02800000086426735 + Shaft Radius: 0.009999999776482582 + Value: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /odom + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: WayPointsMarkers + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /waypoints + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VisualizationMarkers + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: visualization_marker_array + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: BackwardsGlobalPlanMarkers + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: backward_planner/markers + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: BackwardLocalPlannerGoalMarker + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /backward_local_planner/goal_marker + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: ForwardCarrot + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /forward_local_planner/carrot_goal_marker + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: UndoGlobalPlannerMarkers + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /undo_path_planner/markers + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: UndoGlobalPlan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /undo_path_planner/global_plan + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 255; 0 + Enabled: true + Head Diameter: 0.05999999865889549 + Head Length: 0.03999999910593033 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.0010000000474974513 + Name: BackwardLocalPlan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 0 + Pose Style: Arrows + Radius: 0.029999999329447746 + Shaft Diameter: 0.019999999552965164 + Shaft Length: 0.009999999776482582 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /backward_local_planner/path + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 1263.3914794921875 + Min Color: 0; 0; 0 + Min Intensity: 634.1690063476562 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /scan + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - 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/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + - Class: nav2_rviz_plugins/GoalTool + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Angle: -4.659997940063477 + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Scale: -72.154541015625 + Target Frame: base_link + Value: TopDownOrtho (rviz_default_plugins) + X: 0.8163416981697083 + Y: -0.20031072199344635 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 656 + Hide Left Dock: false + Hide Right Dock: true + Navigation 2: + collapsed: false + QMainWindow State: 000000ff00000000fd00000004000000000000028600000236fc020000000bfb0000001200530065006c0065006300740069006f006e000000003d000000830000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000236000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003200000002da000001010000013900fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000002800fffffffb0000001e005200650061006c00730065006e0073006500430061006d0065007200610100000177000000fc0000000000000000000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000022e0000023600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + RealsenseCamera: + collapsed: false + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1210 + X: 70 + Y: 27 diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/rviz/nav2_namespaced_view.rviz b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/rviz/nav2_namespaced_view.rviz new file mode 100644 index 000000000..2a024f75b --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/rviz/nav2_namespaced_view.rviz @@ -0,0 +1,371 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 195 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /RobotModel1/Status1 + - /TF1/Frames1 + - /TF1/Tree1 + - /Global Planner1/Global Costmap1 + Splitter Ratio: 0.5833333134651184 + Tree Height: 464 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: nav2_rviz_plugins/Navigation 2 + Name: Navigation 2 +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 + - 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: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: "" + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Bumper Hit + Position Transformer: "" + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Spheres + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /mobile_base/sensors/bumper_pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: true + Enabled: true + Name: Map + Topic: + Depth: 1 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map + Use Timestamp: false + Value: true + - Alpha: 1 + Arrow Length: 0.019999999552965164 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz_default_plugins/PoseArray + Color: 0; 180; 0 + Enabled: true + Head Length: 0.07000000029802322 + Head Radius: 0.029999999329447746 + Name: Amcl Particle Swarm + Shaft Length: 0.23000000417232513 + Shaft Radius: 0.009999999776482582 + Shape: Arrow (Flat) + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /particlecloud + Value: true + - Class: rviz_common/Group + Displays: + - Alpha: 0.30000001192092896 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Global Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 0; 0 + Enabled: true + Head Diameter: 0.019999999552965164 + Head Length: 0.019999999552965164 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: Arrows + Radius: 0.029999999329447746 + Shaft Diameter: 0.004999999888241291 + Shaft Length: 0.019999999552965164 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /plan + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: false + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/published_footprint + Value: false + Enabled: true + Name: Global Planner + - Class: rviz_common/Group + Displays: + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Local Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 0; 12; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Local Plan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_plan + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Trajectories + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /marker + Value: false + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: true + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/published_footprint + Value: true + Enabled: true + Name: Controller + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - 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 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: nav2_rviz_plugins/GoalTool + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Angle: -1.5700000524520874 + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Scale: 134.638427734375 + Target Frame: + Value: TopDownOrtho (rviz_default_plugins) + X: -0.032615214586257935 + Y: -0.0801941454410553 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 914 + Hide Left Dock: false + Hide Right Dock: true + Navigation 2: + collapsed: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a00000338fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002c4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003201000003070000006e0000006200ffffff000000010000010f00000338fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000338000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004990000033800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1545 + X: 186 + Y: 117 diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/scripts/test_oscillation.py b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/scripts/test_oscillation.py new file mode 100755 index 000000000..3008e133b --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/scripts/test_oscillation.py @@ -0,0 +1,165 @@ +#!/usr/bin/env python3 + +# 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 rclpy +from rclpy.node import Node +from geometry_msgs.msg import Twist +from tf2_ros import TransformListener +from tf2_ros import Buffer +import math +import tf_transformations +import std_msgs.msg + + +class RotationOscillation(Node): + def __init__(self): + super().__init__("rotation_oscillation") + self.cmd_vel_pub = self.create_publisher(Twist, "cmd_vel", 1) + self.yaw_pub = self.create_publisher(std_msgs.msg.Float64, "yaw", 1) + + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + self.on_timer = self.create_timer(0.05, self.on_timer_callback) + + self.base_link_frame = "base_footprint" + self.fixed_frame = "odom" + + def on_timer_callback(self): + try: + base_link_transform = self.tf_buffer.lookup_transform( + self.fixed_frame, self.base_link_frame, rclpy.time.Time() + ) + q = base_link_transform.transform.rotation + euler = tf_transformations.euler_from_quaternion([q.x, q.y, q.z, q.w]) + self.get_logger().info("Current rotation: %f" % math.degrees(euler[2])) + self.get_logger().info("Transform available!") + + t = float(self.get_clock().now().nanoseconds) / 1e9 + cmd_vel = Twist() + cmd_vel.angular.z = 1.0 if math.sin(t) > 0.0 else -1.0 + self.cmd_vel_pub.publish(cmd_vel) + + self.yaw_pub.publish(std_msgs.msg.Float64(data=euler[2])) + + self.get_logger().info("Current time: %s" % str(t)) + except Exception as e: + self.get_logger().error("Error getting transform: %s" % str(e)) + + def oscillate_rotation(self): + rate = self.create_rate(1) + angle = math.radians(25) + # timeout = rclpy.duration.Duration(seconds=1.0) # Timeout for waiting on transforms + + fixed_frame = "odom" + base_link_frame = "base_footprint" + + print(f'Waiting for transform from "{fixed_frame}" to "{base_link_frame}"...') + while rclpy.ok(): + # self.get_logger().info(f'Waiting for transform from "{fixed_frame}" to "{base_link_frame}"...') + rclpy.spin_once(self) + # self.get_logger().info('Spin once, now: %s' % str(self.get_clock().now().toSec())) + rate.sleep() + + self.get_logger().info("Transform available!") + + # Get initial position + print("Getting initial position...") + try: + base_link_transform = self.tf_buffer.lookup_transform( + "map", base_link_frame, rclpy.time.Time() + ) + initial_position = base_link_transform.transform.translation + except Exception as e: + self.get_logger().error("Error getting initial position: %s" % str(e)) + return + + # Rotate 25 degrees to the left + print("Rotating 25 degrees to the left...") + self.get_logger().info("Rotating 25 degrees to the left...") + twist_msg = Twist() + twist_msg.angular.z = angle + self.cmd_vel_pub.publish(twist_msg) + rclpy.spin_once(self) + + # Get position after left rotation + print("Getting position after left rotation...") + try: + print("Getting position after left rotation...") + base_link_transform = self.tf_buffer.lookup_transform( + "map", base_link_frame, rclpy.time.Time() + ) + left_rotation_position = base_link_transform.transform.translation + except Exception as e: + self.get_logger().error("Error getting position after left rotation: %s" % str(e)) + return + + # Rotate 25 degrees to the right + print("Rotating 25 degrees to the right...") + self.get_logger().info("Rotating 25 degrees to the right...") + twist_msg.angular.z = -angle + self.cmd_vel_pub.publish(twist_msg) + rclpy.spin_once(self) + + # Get position after right rotation + try: + base_link_transform = self.tf_buffer.lookup_transform( + "map", base_link_frame, rclpy.time.Time() + ) + right_rotation_position = base_link_transform.transform.translation + except Exception as e: + self.get_logger().error("Error getting position after right rotation: %s" % str(e)) + return + + # Calculate phase difference + print("Calculating phase difference...") + left_rotation_angle = math.atan2( + left_rotation_position.y - initial_position.y, + left_rotation_position.x - initial_position.x, + ) + right_rotation_angle = math.atan2( + right_rotation_position.y - initial_position.y, + right_rotation_position.x - initial_position.x, + ) + phase_difference = math.degrees(right_rotation_angle - left_rotation_angle) + + self.get_logger().info("Phase difference: %f degrees" % phase_difference) + + # Stop the robot + twist_msg.angular.z = 0.0 + self.cmd_vel_pub.publish(twist_msg) + + def shutdown(self): + self.get_logger().info("Shutting down...") + self.destroy_node() + + +def main(args=None): + rclpy.init(args=args) + rotation_oscillation = RotationOscillation() + + rclpy.spin(rotation_oscillation) + + try: + rotation_oscillation.oscillate_rotation() + except Exception as e: + rotation_oscillation.get_logger().error("Error during rotation oscillation: %s" % str(e)) + + rotation_oscillation.shutdown() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/scripts/transform_publisher.py b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/scripts/transform_publisher.py new file mode 100644 index 000000000..8ff606892 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/scripts/transform_publisher.py @@ -0,0 +1,118 @@ +#!/usr/bin/env python3 + +# 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 rclpy +from rclpy.node import Node +import tf2_ros +from geometry_msgs.msg import TransformStamped +import rosgraph_msgs.msg + +tfbrod = None +clock_msg = None +class StaticTransformPublisher: + def __init__(self, node, parent_frame, child_frame, xyz=[0.0, 0.0, 0.0], rpy=[0.0, 0.0, 0.0]): + self.node = node + global tfbrod + print("Initializing static transform publisher") + + self.transform_broadcaster =tfbrod + print("xyz: ", xyz) + + try: + self.transform = TransformStamped() + self.transform.header.frame_id = parent_frame + self.transform.child_frame_id = child_frame + self.transform.transform.translation.x = xyz[0] + self.transform.transform.translation.y = xyz[1] + self.transform.transform.translation.z = xyz[2] + + self.transform.transform.rotation.x = 0.0 + self.transform.transform.rotation.y = 0.0 + self.transform.transform.rotation.z = 0.0 + self.transform.transform.rotation.w = 1.0 + except Exception as e: + print(e) + self.node.get_logger().error("Exception in StaticTransformPublisher: %r" % e) + + self.timer = node.create_timer(0.1, self.publish_transform) + + + def publish_transform(self): + global clock_msg + if clock_msg is None: + return + + time = rclpy.time.Time(seconds = clock_msg.clock.sec, nanoseconds = clock_msg.clock.nanosec) + #time = time + rclpy.time.Duration(seconds=0.01) + self.transform.header.stamp = time.to_msg() + self.transform_broadcaster.sendTransform(self.transform) + self.node.get_logger().info("Publishing transform from %s to %s" % (self.transform.header.frame_id, self.transform.child_frame_id)) + + +def main(args=None): + print("Initializing static transform publisher") + rclpy.init() + + node = rclpy.create_node("static_transform_publisher_1") + + global tfbrod + + tfbrod = tf2_ros.TransformBroadcaster(node) + + def callback(msg): + global clock_msg + clock_msg = msg + + + clock_sub = node.create_subscription( + rosgraph_msgs.msg.Clock, + "/clock", + callback, + qos_profile=rclpy.qos.qos_profile_sensor_data, + ) + + print("Creating static transform publisher nodes") + # Create the first instance of StaticTransformPublisher + node1 = StaticTransformPublisher( + node, + "base_footprint", + "base_link", + ) + + # + # Create the second instance of StaticTransformPublisher + node2 = StaticTransformPublisher( + node, + "base_link", + "base_scan", + xyz=[-0.064, 0.0, 0.122], + ) + + # node3 = StaticTransformPublisher( + # node, + # "map", + # "odom" + # ) + + rclpy.spin(node) + rclpy.shutdown() + + +if __name__ == "__main__": + import sys + print("Starting static transform publisher") + + main() diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/servers/led_action_server/action/LEDControl.action b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/servers/led_action_server/action/LEDControl.action new file mode 100644 index 000000000..e4dc1d976 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/servers/led_action_server/action/LEDControl.action @@ -0,0 +1,18 @@ +uint8 command + +# Possible command values +uint8 CMD_ON = 0 +uint8 CMD_OFF = 1 +--- +uint8 state + +# Possible feeedback values (tool states) +uint8 STATE_UNKNOWN = 0 +uint8 STATE_RUNNING = 1 +uint8 STATE_IDLE = 2 +--- +uint8 result + +# Possible result values +uint8 RESULT_SUCCESS = 0 +uint8 RESULT_ERROR = 1 diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/servers/led_action_server/src/led_action_server_node.cpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/servers/led_action_server/src/led_action_server_node.cpp new file mode 100644 index 000000000..1ac313ce1 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/servers/led_action_server/src/led_action_server_node.cpp @@ -0,0 +1,230 @@ +// 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 + * + ******************************************************************************************************************/ +// #include +// #include +// #include +// #include + +#include +#include +#include +#include +#include +// #include +// #include + +#include +#include +#include + +// This class describes a preemptable-on/off tool action server to be used from smacc +// shows in rviz a sphere whose color describe the current state (unknown, running, idle) +class LEDActionServer : public rclcpp::Node +{ +public: + std::shared_ptr> as_; + using GoalHandleLEDControl = + rclcpp_action::ServerGoalHandle; + + rclcpp::Publisher::SharedPtr stateMarkerPublisher_; + + uint8_t cmd; + + uint8_t currentState_; + + /** +****************************************************************************************************************** +* constructor() +****************************************************************************************************************** +*/ + LEDActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) + : Node("led_action_server_node", options) + { + currentState_ = sm_dancebot_artgallery_ue::action::LEDControl::Result::STATE_UNKNOWN; + } + + /** +****************************************************************************************************************** +* execute() +****************************************************************************************************************** +*/ + void execute(const std::shared_ptr + gh) // Note: "Action" is not appended to DoDishes here + { + auto goal = gh->get_goal(); + RCLCPP_INFO_STREAM(get_logger(), "Tool action server request: " << goal->command); + + cmd = goal->command; + + if (goal->command == sm_dancebot_artgallery_ue::action::LEDControl_Goal::CMD_ON) + { + RCLCPP_INFO(this->get_logger(), "ON"); + currentState_ = sm_dancebot_artgallery_ue::action::LEDControl_Result::STATE_RUNNING; + } + else if (goal->command == sm_dancebot_artgallery_ue::action::LEDControl_Goal::CMD_OFF) + { + RCLCPP_INFO(this->get_logger(), "OFF"); + currentState_ = sm_dancebot_artgallery_ue::action::LEDControl_Result::STATE_IDLE; + } + + auto feedback_msg = std::make_shared(); + + // 10Hz internal loop + rclcpp::Rate rate(20); + + while (rclcpp::ok()) + { + gh->publish_feedback(feedback_msg); + + publishStateMarker(); + rate.sleep(); + RCLCPP_INFO_THROTTLE(this->get_logger(), *(this->get_clock()), 2000, "Loop feedback"); + } + + auto result = std::make_shared(); + result->state = this->currentState_; + + // never reach succeeded because were are interested in keeping the feedback alive + //as_->setSucceeded(); + gh->succeed(result); + } + + rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID & /*uuid*/, + std::shared_ptr /*goal*/) + { + // (void)uuid; + // // Let's reject sequences that are over 9000 + // if (goal->order > 9000) { + // return rclcpp_action::GoalResponse::REJECT; + // } + + // lets accept everything + RCLCPP_INFO(this->get_logger(), "Handle goal"); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } + + rclcpp_action::CancelResponse handle_cancel( + const std::shared_ptr /*goal_handle*/) + { + RCLCPP_INFO(rclcpp::get_logger("server"), "Got request to cancel goal"); + //(void)goal_handle; + return rclcpp_action::CancelResponse::ACCEPT; + } + + void handle_accepted(const std::shared_ptr goal_handle) + { + // this needs to return quickly to avoid blocking the executor, so spin up a new thread + std::thread{std::bind(&LEDActionServer::execute, this, std::placeholders::_1), goal_handle} + .detach(); + } + + /** +****************************************************************************************************************** +* run() +****************************************************************************************************************** +*/ + void run() + { + RCLCPP_INFO(this->get_logger(), "Creating tool action server"); + //as_ = std::make_shared(n, "led_action_server", boost::bind(&LEDActionServer::execute, this, _1), false); + + this->as_ = rclcpp_action::create_server( + this, "led_action_server", + std::bind(&LEDActionServer::handle_goal, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&LEDActionServer::handle_cancel, this, std::placeholders::_1), + std::bind(&LEDActionServer::handle_accepted, this, std::placeholders::_1)); + + RCLCPP_INFO(get_logger(), "Starting Tool Action Server"); + stateMarkerPublisher_ = + this->create_publisher("tool_markers", 1); + } + + /** +****************************************************************************************************************** +* publishStateMarker() +****************************************************************************************************************** +*/ + void publishStateMarker() + { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "base_link"; + marker.header.stamp = this->now(); + + marker.ns = "tool_namespace"; + marker.id = 0; + marker.type = visualization_msgs::msg::Marker::SPHERE; + marker.action = visualization_msgs::msg::Marker::ADD; + + marker.scale.x = 0.2; + marker.scale.y = 0.2; + marker.scale.z = 0.2; + + marker.color.a = 1; + + if (currentState_ == sm_dancebot_artgallery_ue::action::LEDControl::Result::STATE_RUNNING) + { + // show green ball + marker.color.r = 0; + marker.color.g = 1; + marker.color.b = 0; + } + else if (currentState_ == sm_dancebot_artgallery_ue::action::LEDControl::Result::STATE_IDLE) + { + // show gray ball + marker.color.r = 0.7; + marker.color.g = 0.7; + marker.color.b = 0.7; + } + else + { + // show black ball + marker.color.r = 0; + marker.color.g = 0; + marker.color.b = 0; + } + + marker.pose.orientation.w = 1; + marker.pose.position.x = 0; + marker.pose.position.y = 0; + marker.pose.position.z = 1; + + visualization_msgs::msg::MarkerArray ma; + ma.markers.push_back(marker); + + stateMarkerPublisher_->publish(ma); + } +}; + +/** +****************************************************************************************************************** +* main() +****************************************************************************************************************** +*/ +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto ledactionserver = std::make_shared(); + ledactionserver->run(); + + rclcpp::spin(ledactionserver); + rclcpp::shutdown(); + return 0; +} diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/servers/service_node_3/service_node_3.py b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/servers/service_node_3/service_node_3.py new file mode 100644 index 000000000..a6694cde3 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/servers/service_node_3/service_node_3.py @@ -0,0 +1,47 @@ +#!/usr/bin/env python3 + +# 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. + +# import roslib +# import rospy +import rclpy +from rclpy.node import Node + +import std_srvs +import std_srvs.srv + +if __name__ == "__main__": + + class Service3(Node): + def __init__(self): + super().__init__("Service3") + self.s = self.create_service( + std_srvs.srv.SetBool, "service_node3", self.set_bool_service + ) + + def set_bool_service(self, req, res): + self.get_logger().info("RECEIVING SET BOOL SERVICE REQUEST: value=" + str(req.data)) + + res.message = "OK, value set" + res.success = True + return res + + rclpy.init(args=None) + s = Service3() + + # s = rospy.Service('service_node3', std_srvs.srv.SetBool, set_bool_service) + # rospy.spin() + rclpy.spin(s) + rclpy.shutdown() diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/servers/temperature_sensor_node/src/temperature_sensor_node.cpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/servers/temperature_sensor_node/src/temperature_sensor_node.cpp new file mode 100644 index 000000000..ce66d640a --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/servers/temperature_sensor_node/src/temperature_sensor_node.cpp @@ -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. + +#include +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + + auto nh = rclcpp::Node::make_shared("temperature_sensor_node"); + auto pub = nh->create_publisher("/temperature", 1); + + rclcpp::Rate r(10); + + while (rclcpp::ok()) + { + sensor_msgs::msg::Temperature msg; + msg.temperature = sin(nh->now().seconds() / 2.0) * 50; + pub->publish(msg); + + r.sleep(); + rclcpp::spin_some(nh); + } +} diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/src/sm_dancebot_artgalery_ue/clients/client_behaviors/nav2z_client/cb_active_stop.cpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/src/sm_dancebot_artgalery_ue/clients/client_behaviors/nav2z_client/cb_active_stop.cpp new file mode 100644 index 000000000..cc14fe162 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/src/sm_dancebot_artgalery_ue/clients/client_behaviors/nav2z_client/cb_active_stop.cpp @@ -0,0 +1,53 @@ + +// 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 + * + ******************************************************************************************************************/ + +#include +#include +#include + +#include +#include + +namespace sm_dancebot_artgallery_ue +{ +CbActiveStop::CbActiveStop() {} + +void CbActiveStop::onEntry() +{ + auto nh = this->getNode(); + cmd_vel_pub_ = nh->create_publisher("/cmd_vel", rclcpp::QoS(1)); + + rclcpp::Rate loop_rate(5); + geometry_msgs::msg::Twist cmd_vel_msg; + while (!this->isShutdownRequested()) + { + cmd_vel_msg.linear.x = 0; + cmd_vel_msg.angular.z = 0; + + cmd_vel_pub_->publish(cmd_vel_msg); + loop_rate.sleep(); + } + RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] Finished behavior execution"); + + this->postSuccessEvent(); +} + +void CbActiveStop::onExit() {} + +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/src/sm_dancebot_artgallery_ue/clients/client_behaviors/nav2z_client/cb_active_stop.cpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/src/sm_dancebot_artgallery_ue/clients/client_behaviors/nav2z_client/cb_active_stop.cpp new file mode 100644 index 000000000..cc14fe162 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/src/sm_dancebot_artgallery_ue/clients/client_behaviors/nav2z_client/cb_active_stop.cpp @@ -0,0 +1,53 @@ + +// 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 + * + ******************************************************************************************************************/ + +#include +#include +#include + +#include +#include + +namespace sm_dancebot_artgallery_ue +{ +CbActiveStop::CbActiveStop() {} + +void CbActiveStop::onEntry() +{ + auto nh = this->getNode(); + cmd_vel_pub_ = nh->create_publisher("/cmd_vel", rclcpp::QoS(1)); + + rclcpp::Rate loop_rate(5); + geometry_msgs::msg::Twist cmd_vel_msg; + while (!this->isShutdownRequested()) + { + cmd_vel_msg.linear.x = 0; + cmd_vel_msg.angular.z = 0; + + cmd_vel_pub_->publish(cmd_vel_msg); + loop_rate.sleep(); + } + RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] Finished behavior execution"); + + this->postSuccessEvent(); +} + +void CbActiveStop::onExit() {} + +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/src/sm_dancebot_artgallery_ue/clients/client_behaviors/nav2z_client/cb_position_control_free_space.cpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/src/sm_dancebot_artgallery_ue/clients/client_behaviors/nav2z_client/cb_position_control_free_space.cpp new file mode 100644 index 000000000..5ffd9df29 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/src/sm_dancebot_artgallery_ue/clients/client_behaviors/nav2z_client/cb_position_control_free_space.cpp @@ -0,0 +1,195 @@ +// 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 + * + ******************************************************************************************************************/ + +#include +#include +#include + +#include +#include + +namespace sm_dancebot_artgallery_ue +{ +CbPositionControlFreeSpace::CbPositionControlFreeSpace() +: targetYaw_(0), k_betta_(1.0), max_angular_yaw_speed_(1.0) +{ +} + +void CbPositionControlFreeSpace::updateParameters() {} + +void CbPositionControlFreeSpace::onEntry() +{ + auto nh = this->getNode(); + cmd_vel_pub_ = nh->create_publisher("/cmd_vel", rclcpp::QoS(1)); + + sm_dancebot_artgallery_ue::CpUEPose * pose; + this->requiresComponent(pose); + + geometry_msgs::msg::Twist cmd_vel; + goalReached_ = false; + + geometry_msgs::msg::Pose currentPose = pose->toPoseMsg(); + + rclcpp::Rate loop_rate(10); + double countAngle = 0; + auto prevyaw = tf2::getYaw(currentPose.orientation); + + // PID controller gains (proportional, integral, and derivative) + double kp_linear = 0.5; + double ki_linear = 0.0; + double kd_linear = 0.1; + + double kp_angular = 0.5; + double ki_angular = 0.0; + double kd_angular = 0.1; + + while (rclcpp::ok() && !goalReached_) + { + RCLCPP_INFO_STREAM_THROTTLE( + getLogger(), *getNode()->get_clock(), 200, + "CbPositionControlFreeSpace, current pose: " << currentPose.position.x << ", " + << currentPose.position.y << ", " + << tf2::getYaw(currentPose.orientation)); + + RCLCPP_INFO_STREAM_THROTTLE( + getLogger(), *getNode()->get_clock(), 200, + "CbPositionControlFreeSpace, target pose: " << target_pose_.position.x << ", " + << target_pose_.position.y << ", " + << tf2::getYaw(target_pose_.orientation)); + + tf2::Quaternion q; + currentPose = pose->toPoseMsg(); + + // Here we implement the control logic to calculate the velocity command + // based on the received position of the vehicle and the target pose. + + // Calculate the errors in x and y + double error_x = target_pose_.position.x - currentPose.position.x; + double error_y = target_pose_.position.y - currentPose.position.y; + + // Calculate the distance to the target pose + double distance_to_target = std::sqrt(error_x * error_x + error_y * error_y); + + RCLCPP_INFO_STREAM( + getLogger(), "[" << getName() << "] distance to target: " << distance_to_target + << " ( th: " << threshold_distance_ << ")"); + + // Check if the robot has reached the target pose + if (distance_to_target < threshold_distance_) + { + RCLCPP_INFO(getLogger(), "Goal reached!"); + // Stop the robot by setting the velocity commands to zero + geometry_msgs::msg::Twist cmd_vel_msg; + cmd_vel_msg.linear.x = 0.0; + cmd_vel_msg.angular.z = 0.0; + cmd_vel_pub_->publish(cmd_vel_msg); + break; + } + else + { + // Calculate the desired orientation angle + double desired_yaw = std::atan2(error_y, error_x); + + // Calculate the difference between the desired orientation and the current orientation + double yaw_error = desired_yaw - (tf2::getYaw(currentPose.orientation) + M_PI ); + + // Ensure the yaw error is within the range [-pi, pi] + while (yaw_error > M_PI) yaw_error -= 2 * M_PI; + while (yaw_error < -M_PI) yaw_error += 2 * M_PI; + + // Calculate the control signals (velocity commands) using PID controllers + double cmd_linear_x = kp_linear * distance_to_target + ki_linear * integral_linear_ + + kd_linear * (distance_to_target - prev_error_linear_); + double cmd_angular_z = kp_angular * yaw_error + ki_angular * integral_angular_ + + kd_angular * (yaw_error - prev_error_angular_); + + + if (cmd_linear_x > max_linear_velocity) + cmd_linear_x = max_linear_velocity; + else if (cmd_linear_x < -max_linear_velocity) + cmd_linear_x = -max_linear_velocity; + + if (cmd_angular_z > max_angular_velocity) + cmd_angular_z = max_angular_velocity; + else if (cmd_angular_z < -max_angular_velocity) + cmd_angular_z = -max_angular_velocity; + + // Construct and publish the velocity command message + geometry_msgs::msg::Twist cmd_vel_msg; + + cmd_vel_msg.linear.x = cmd_linear_x; + cmd_vel_msg.angular.z = cmd_angular_z; + + cmd_vel_pub_->publish(cmd_vel_msg); + + // Update errors and integrals for the next control cycle + prev_error_linear_ = distance_to_target; + prev_error_angular_ = yaw_error; + integral_linear_ += distance_to_target; + integral_angular_ += yaw_error; + + // tf2::fromMsg(currentPose.orientation, q); + // auto currentYaw = tf2::getYaw(currentPose.orientation); + // auto deltaAngle = angles::shortest_angular_distance(prevyaw, currentYaw); + // countAngle += deltaAngle; + + // prevyaw = currentYaw; + // double angular_error = targetYaw_ - countAngle; + + // auto omega = angular_error * k_betta_; + // cmd_vel.linear.x = 0; + // cmd_vel.linear.y = 0; + // cmd_vel.linear.z = 0; + // cmd_vel.angular.z = + // std::min(std::max(omega, -fabs(max_angular_yaw_speed_)), fabs(max_angular_yaw_speed_)); + + // RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] delta angle: " << deltaAngle); + // RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] cummulated angle: " << countAngle); + // RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] k_betta_: " << k_betta_); + + // RCLCPP_INFO_STREAM( + // getLogger(), "[" << getName() << "] angular error: " << angular_error << "(" + // << yaw_goal_tolerance_rads_ << ")"); + // RCLCPP_INFO_STREAM( + // getLogger(), "[" << getName() << "] command angular speed: " << cmd_vel.angular.z); + + // if (fabs(angular_error) < yaw_goal_tolerance_rads_) + // { + // RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] GOAL REACHED. Sending stop command."); + // goalReached_ = true; + // cmd_vel.linear.x = 0; + // cmd_vel.angular.z = 0; + // break; + // } + + // this->cmd_vel_pub_->publish(cmd_vel); + + loop_rate.sleep(); + } + } + + RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] Finished behavior execution"); + + this->postSuccessEvent(); +} + +void CbPositionControlFreeSpace::onExit() {} + +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/src/sm_dancebot_artgallery_ue/clients/components/cp_ue_pose.cpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/src/sm_dancebot_artgallery_ue/clients/components/cp_ue_pose.cpp new file mode 100644 index 000000000..0dc99bb63 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/src/sm_dancebot_artgallery_ue/clients/components/cp_ue_pose.cpp @@ -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. + +/***************************************************************************************************************** + *-2020 + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace sm_dancebot_artgallery_ue +{ +CpUEPose::CpUEPose(std::string topicname) + : CpTopicSubscriber(topicname) +{ +} + +void CpUEPose::onInitialize() +{ + CpTopicSubscriber::onInitialize(); + this->onMessageReceived(&CpUEPose::onPoseMessageReceived, this); +} + +void CpUEPose::onPoseMessageReceived(const ue_msgs::msg::EntityState& msg) +{ + this->entityStateMsg_ = msg;; + + RCLCPP_INFO_STREAM_THROTTLE(getLogger(), *(getNode()->get_clock()), 200, "Received UEPose x: " << msg.pose.position.x << " y: " << msg.pose.position.y << " z: " << msg.pose.position.z); +} + +geometry_msgs::msg::Pose CpUEPose::toPoseMsg() +{ + return this->entityStateMsg_.pose; +} + +} // namespace cl_nav2z diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/src/sm_dancebot_artgallery_ue/sm_dancebot_artgallery_ue.cpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/src/sm_dancebot_artgallery_ue/sm_dancebot_artgallery_ue.cpp new file mode 100644 index 000000000..8d9c457ef --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/src/sm_dancebot_artgallery_ue/sm_dancebot_artgallery_ue.cpp @@ -0,0 +1,27 @@ +// 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 + * + ******************************************************************************************************************/ + +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + smacc2::run(); +} diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/urdf/turtlebot3_waffle.urdf b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/urdf/turtlebot3_waffle.urdf new file mode 100644 index 000000000..f4307672c --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/urdf/turtlebot3_waffle.urdf @@ -0,0 +1,293 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/worlds/ridgeback_race.world b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/worlds/ridgeback_race.world new file mode 100644 index 000000000..3ff47663c --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/worlds/ridgeback_race.world @@ -0,0 +1,4864 @@ + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + 0 + 0 + 0 + + + + 1 + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + + + + + + + + 10 + + + 0 + + + 0 0 1 + 100 100 + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + + + 0.001 + 1 + 1000 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + EARTH_WGS84 + 0 + 0 + 0 + 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -1 -6 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -4 1.55828 0 0 -0 0 + + + 3116 755000000 + 356 954966630 + 1605650970 323926707 + 355029 + + -2.6481 -4.01574 -0.045465 0 0 -0.74752 + 1 1 1 + + 8.54538 -6.87025 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.25439 -7.04543 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 1.49008 -7.104 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.8196 -6.78892 -0.045465 0 0 -0.74752 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.6481 -4.01574 -0.045465 0 0 -2.31822 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.78254 -6.92811 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 5.0182 -6.98712 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.8196 -6.78892 0.820535 0 0 -0.74752 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -0.070844 -6.42684 -0.045465 0 0 -2.31822 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1.3598 -5.22163 -0.045465 0 0 -2.31822 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -15.3977 -1.55916 0 0 -0 1.04571 + 1 1 1 + + -15.0829 9.98827 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.7448 4.86628 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.2984 3.15835 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -15.4433 11.2132 0 0 -0 1.04571 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -15.3977 -1.55916 0 0 0 -0.524988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.6375 8.28162 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.1908 6.57377 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -15.4433 11.2132 0.866 0 -0 1.04571 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.6146 1.4865 0 0 0 -0.524988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.5057 -0.036576 0 0 0 -0.524988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -2.60683 -10.2645 0 0 0 -0.76918 + 1 1 1 + + 8.5222 -13.3608 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.22866 -13.4213 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 1.46349 -13.4417 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.79789 -13.3071 0 0 0 -0.76918 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.60683 -10.2645 0 0 0 -2.33988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.75852 -13.3804 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 4.99332 -13.4012 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.79789 -13.3071 0.866 0 0 -0.76918 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -0.082399 -12.7309 0 0 0 -2.33988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1.34495 -11.498 0 0 0 -2.33988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3347 -5.65314 0 0 -0 1.56441 + 1 1 1 + + 4.88352 4.5315 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 8.58481 0.746571 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.81915 -0.515431 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.96327 5.4166 0 0 -0 1.56441 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.3347 -5.65314 0 0 0 -0.006293 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.11634 3.2701 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 7.35101 2.00838 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.96327 5.4166 0.866 0 -0 1.56441 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.3734 -2.1241 0 0 0 -0.006293 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.3545 -3.88863 0 0 0 -0.006293 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -16.0335 -6.41848 0 0 0 -0.7879 + 1 1 1 + + -4.96438 -9.72253 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -10.2581 -9.68398 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -12.0234 -9.67127 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -3.68791 -9.69271 0 0 0 -0.7879 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -16.0335 -6.41848 0 0 0 -2.3586 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -6.72812 -9.70918 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -8.4934 -9.69692 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -3.68791 -9.69271 0.866 0 0 -0.7879 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.5557 -8.93166 0 0 0 -2.3586 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.7949 -7.6754 0 0 0 -2.3586 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -1 16 0 0 -0 0 + 1 1 1 + + 9.2248 10.3902 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 5.49818 14.15 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 4.2559 15.4033 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.0288 9.55797 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1 16 0 0 0 -1.5707 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 7.98256 11.6439 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.74031 12.8974 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.0288 9.55797 0.866 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 2.52922 15.9838 0 0 0 -1.5707 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 0.7646 15.9914 0 0 0 -1.5707 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -14.6681 12.3809 0 0 -0 0.810038 + 1 1 1 + + -3.55516 15.9191 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -8.84779 15.8121 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -10.6121 15.7764 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.39802 15.9276 0 0 -0 0.810038 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.6681 12.3809 0 0 0 -0.760662 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -5.3197 15.8837 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -7.08412 15.8482 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.39802 15.9276 0.866 0 -0 0.810038 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -12.2231 14.926 0 0 0 -0.760662 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.4453 13.6531 0 0 0 -0.760662 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -8.62781 9.94618 0 0 -0 0.2056 + 1 1 1 + + 2.5269 6.54195 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1.88879 9.46171 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -3.36079 10.4351 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.48387 5.89136 0 0 -0 0.205596 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -8.62781 9.94618 0 0 0 -1.3651 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 1.05488 7.51562 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -0.41711 8.4891 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.48387 5.89136 0.866 0 -0 0.205596 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -5.16961 10.6508 0 0 0 -1.3651 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -6.89862 10.298 0 0 0 -1.3651 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -16.7548 -4.64388 0 0 -0 0 + 1 1 1 + + -16.7548 -4.64388 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -17.1282 -4.13814 0 0 -0 0 + 1 1 1 + + -17.1282 -4.13814 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3218 -9.21435 0 0 -0 0 + 1 1 1 + + 10.3218 -9.21435 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3067 -12.7762 0 0 -0 0 + 1 1 1 + + 10.3067 -12.7762 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3176 -7.4365 0 0 -0 0 + 1 1 1 + + 10.3176 -7.4365 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3136 -11.0025 0 0 -0 0 + 1 1 1 + + 10.3136 -11.0025 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -16.2875 -3.11229 0 0 0 -0.509254 + 1 1 1 + + -16.2875 -3.11229 0 0 0 -0.509254 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -17.3228 -5.14571 0 0 0 -2.31969 + 1 1 1 + + -17.3228 -5.14571 0 0 0 -2.31969 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 0 0 -0 0 + 1 1 1 + + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0.127058 -0.012625 0.008401 -5.7e-05 0.003353 -0.002954 + 1 1 1 + + 0.127058 -0.012625 0.008401 -5.7e-05 0.003353 -0.002954 + 0.001954 -0.000866 -0.061964 -0.008321 0.009978 -0.001386 + 0.001244 0.001765 0.002452 -0.135997 -0.01007 0.000878 + 0.001244 0.001765 0.002452 0 -0 0 + + + 0.127066 -0.012614 0.008412 -2.8e-05 0.003314 -0.002936 + 0.001162 -0.002502 -0.062977 -0.012557 0.01357 -0.00402 + -0.002239 0.004917 -0.002962 -0.159433 0.012841 0.004083 + -0.002239 0.004917 -0.002962 0 -0 0 + + + 0.127057 -0.012617 0.0084 -6.9e-05 0.003501 -0.00297 + 0.002415 -0.00193 -0.061395 -0.005008 -0.011677 0.001501 + -0.003148 0.006146 -0.003368 -0.028835 -0.016126 -0.002503 + -0.000393 0.000768 -0.000421 0 -0 0 + + + 0.127059 -0.012617 0.008403 -5.8e-05 0.003458 -0.002962 + 0.002127 -0.002056 -0.061757 -0.007075 -0.00588 0.000236 + -0.001696 0.001569 -0.002879 -0.058616 -0.009069 -0.001539 + -0.001696 0.001569 -0.002879 0 -0 0 + + + -0.049765 0.051908 0.005015 -0.045565 -1.47685 -1.24358 + 0.002355 -0.002164 -0.063759 1.02255 1.31025 -0 + -0.00117 0.003147 -0.007182 0 -0 0 + -1e-06 3e-06 -7e-06 0 -0 0 + + + -0.050165 -0.076075 0.004993 1.90095 0.495873 2.6548 + 0.005449 -0.004628 -0.024591 0.926043 1.08964 0 + -0.023688 0.021829 0.247248 1.35553 -0.826371 3.14159 + -2.4e-05 2.2e-05 0.000247 0 -0 0 + + + 0.060947 -0.012566 -19432.4 0 -0 0 + 0 0 -617.145 0 -0 0 + 0 0 -9.8 0 -0 0 + 0 0 -9.8 0 -0 0 + + + 0.106509 -0.01259 0.041208 3.13937 1.12267 3.13728 + 0.002393 -0.00053 -0.06195 0.008576 0.03027 0.007711 + -0.002374 0.001791 -0.006185 -0.054285 -0.072005 -0.002259 + -0.000237 0.000179 -0.000618 0 -0 0 + + + 0.106061 -0.012586 0.040984 3.13916 1.14848 3.13713 + 0.000902 -0.000269 -0.06003 0.004974 0.030614 0.00275 + -0.001844 0.001728 0.003565 -0.052403 -0.057989 -0.04024 + -0.000184 0.000173 0.000357 0 -0 0 + + + + 0 0 10 0 -0 0 + + + + + 1.03059 2.69165 18.1021 -0 1.5578 -3.12116 + orbit + perspective + + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + 10 -9.57988 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + 10.4084 -11 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + 10.4143 -13 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -1.5566 -14 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -1 -1 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.2559 -0.59667 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.49818 -1.85004 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.74031 -3.10261 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.98256 -4.3561 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.2248 -5.60978 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -1 16 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -14.5593 -16 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.2559 -0.59667 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.49818 -1.85004 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.74031 -3.10261 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.98256 -4.3561 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.2248 -5.60978 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + 25 -15 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -25 -4 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -16.4458 -4 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -17.5466 -5 0 0 -0 0 + + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -17 -4.51853 0 0 -0 0 + + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -17.425 -4 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.2559 -0.59667 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.49818 -1.85004 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.74031 -3.10261 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.98256 -4.3561 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.2248 -5.60978 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -9 10 0 0 -0 0 + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + + -2 -0.5 0.01 0 -0 0 + + 0 + + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + 1 + + 0 + 0 + + + + -0.064 0 0.048 0 -0 0 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + 1 + + + -0.064 0 0.048 0 -0 0 + + + 0.265 0.265 0.089 + + + + + + + + + + + + + + + 10 + + + -0.064 0 0 0 -0 0 + + + model://turtlebot3_waffle/meshes/waffle_base.dae + 0.001 0.001 0.001 + + + + 0 + 0 + 0 + + + + 1 + 200 + + + + + 0 + 0.0002 + + + + + 0 + 0.0002 + + + + + 0 + 0.0002 + + + + + + + 0 + 0.017 + + + + + 0 + 0.017 + + + + + 0 + 0.017 + + + + + + + ~/out:=imu + + + + 0 + + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + 1 + + 0 + 0 + + + + -0.052 0 0.111 0 -0 0 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + 0.125 + + + -0.052 0 0.111 0 -0 0 + + + 0.0508 + 0.055 + + + + + + + + + + + + + + + 10 + + + -0.064 0 0.121 0 -0 0 + + + model://turtlebot3_waffle/meshes/lds.dae + 0.001 0.001 0.001 + + + + + 1 + 1 + -0.064 0 0.121 0 -0 0 + 5 + + + + 360 + 1 + 0 + 6.28 + + + 1 + 0 + 0 + + + + 0.32 + 20.5 + 0.015 + + + gaussian + 0 + 0.01 + + + + + ~/out:=scan + + sensor_msgs/LaserScan + base_scan + + + 0 + 0 + 0 + + + + 0 0.144 0.023 -1.57 0 0 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + 0.1 + + + 0 0.144 0.023 -1.57 0 0 + + + 0.033 + 0.018 + + + + + + 100000 + 100000 + 0 0 0 + 0 + 0 + + + + + + + + 0 + 0.2 + 100000 + 1 + 0.01 + 0.001 + + + + + 10 + + + 0 0.144 0.023 0 -0 0 + + + model://turtlebot3_waffle/meshes/left_tire.dae + 0.001 0.001 0.001 + + + + 0 + 0 + 0 + + + + 0 -0.144 0.023 -1.57 0 0 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + 0.1 + + + 0 -0.144 0.023 -1.57 0 0 + + + 0.033 + 0.018 + + + + + + 100000 + 100000 + 0 0 0 + 0 + 0 + + + + + + + + 0 + 0.2 + 100000 + 1 + 0.01 + 0.001 + + + + + 10 + + + 0 -0.144 0.023 0 -0 0 + + + model://turtlebot3_waffle/meshes/right_tire.dae + 0.001 0.001 0.001 + + + + 0 + 0 + 0 + + + -0.177 -0.064 -0.004 0 -0 0 + + 0.001 + + 1e-05 + 0 + 0 + 1e-05 + 0 + 1e-05 + + 0 0 0 0 -0 0 + + + + + 0.005 + + + + + + 0 + 0.2 + 100000 + 1 + 0.01 + 0.001 + + + + + + + + + + + 10 + + 0 + 0 + 0 + + + -0.177 0.064 -0.004 0 -0 0 + + 0.001 + + 1e-05 + 0 + 0 + 1e-05 + 0 + 1e-05 + + 0 0 0 0 -0 0 + + + + + 0.005 + + + + + + 0 + 0.2 + 100000 + 1 + 0.01 + 0.001 + + + + + + + + + + + 10 + + 0 + 0 + 0 + + + + 0.069 -0.047 0.107 0 -0 0 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + 0.035 + + + 0 0.047 -0.005 0 -0 0 + + + 0.008 0.13 0.022 + + + + + + + + + + + + + + + 10 + + 0.069 -0.047 0.107 0 -0 0 + + 1 + 5 + 0.064 -0.047 0.107 0 -0 0 + + 1.047 + + 320 + 240 + + + 0.1 + 100 + + + + + intel_realsense_r200_depth + camera_depth_frame + 0.07 + 0.001 + 5.0 + + + + 0 0.047 -0.005 0 -0 0 + + + 0.008 0.13 0.022 + + + + + + + + + + + + + + + 10 + + 0.069 -0.047 0.107 0 -0 0 + 0 + 0 + 0 + + + base_footprint + base_link + 0 0 0.01 0 -0 0 + + + base_link + wheel_left_link + 0 0.144 0.023 -1.57 0 0 + + 0 0 1 + + -1e+16 + 1e+16 + + + 0 + 0 + + + + + base_link + wheel_right_link + 0 -0.144 0.023 -1.57 0 0 + + 0 0 1 + + -1e+16 + 1e+16 + + + 0 + 0 + + + + + base_link + caster_back_right_link + + + base_link + caster_back_left_link + + + base_link + base_scan + -0.064 0 0.121 0 -0 0 + + 0 0 1 + + -1e+16 + 1e+16 + + + + + base_link + camera_link + 0.064 -0.065 0.094 0 -0 0 + + 0 0 1 + + -1e+16 + 1e+16 + + + + + + + 30 + wheel_left_joint + wheel_right_joint + 0.287 + 0.066 + 20 + 1.0 + cmd_vel + 1 + 1 + 0 + odom + odom + base_footprint + + + + ~/out:=joint_states + + 30 + wheel_left_joint + wheel_right_joint + + + + diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/worlds/ridgeback_race_empty.world b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/worlds/ridgeback_race_empty.world new file mode 100644 index 000000000..3ebb28ab2 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/worlds/ridgeback_race_empty.world @@ -0,0 +1,4193 @@ + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + 0 + 0 + 0 + + + + 1 + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + + + + + + + + 10 + + + 0 + + + 0 0 1 + 100 100 + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + + + 0.001 + 1 + 1000 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + EARTH_WGS84 + 0 + 0 + 0 + 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -1 -6 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -4 1.55828 0 0 -0 0 + + + 3116 755000000 + 356 954966630 + 1605650970 323926707 + 355029 + + -2.6481 -4.01574 -0.045465 0 0 -0.74752 + 1 1 1 + + 8.54538 -6.87025 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.25439 -7.04543 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 1.49008 -7.104 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.8196 -6.78892 -0.045465 0 0 -0.74752 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.6481 -4.01574 -0.045465 0 0 -2.31822 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.78254 -6.92811 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 5.0182 -6.98712 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.8196 -6.78892 0.820535 0 0 -0.74752 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -0.070844 -6.42684 -0.045465 0 0 -2.31822 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1.3598 -5.22163 -0.045465 0 0 -2.31822 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -15.3977 -1.55916 0 0 -0 1.04571 + 1 1 1 + + -15.0829 9.98827 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.7448 4.86628 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.2984 3.15835 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -15.4433 11.2132 0 0 -0 1.04571 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -15.3977 -1.55916 0 0 0 -0.524988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.6375 8.28162 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.1908 6.57377 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -15.4433 11.2132 0.866 0 -0 1.04571 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.6146 1.4865 0 0 0 -0.524988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.5057 -0.036576 0 0 0 -0.524988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -2.60683 -10.2645 0 0 0 -0.76918 + 1 1 1 + + 8.5222 -13.3608 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.22866 -13.4213 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 1.46349 -13.4417 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.79789 -13.3071 0 0 0 -0.76918 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.60683 -10.2645 0 0 0 -2.33988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.75852 -13.3804 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 4.99332 -13.4012 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.79789 -13.3071 0.866 0 0 -0.76918 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -0.082399 -12.7309 0 0 0 -2.33988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1.34495 -11.498 0 0 0 -2.33988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3347 -5.65314 0 0 -0 1.56441 + 1 1 1 + + 4.88352 4.5315 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 8.58481 0.746571 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.81915 -0.515431 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.96327 5.4166 0 0 -0 1.56441 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.3347 -5.65314 0 0 0 -0.006293 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.11634 3.2701 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 7.35101 2.00838 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.96327 5.4166 0.866 0 -0 1.56441 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.3734 -2.1241 0 0 0 -0.006293 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.3545 -3.88863 0 0 0 -0.006293 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -16.0335 -6.41848 0 0 0 -0.7879 + 1 1 1 + + -4.96438 -9.72253 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -10.2581 -9.68398 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -12.0234 -9.67127 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -3.68791 -9.69271 0 0 0 -0.7879 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -16.0335 -6.41848 0 0 0 -2.3586 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -6.72812 -9.70918 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -8.4934 -9.69692 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -3.68791 -9.69271 0.866 0 0 -0.7879 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.5557 -8.93166 0 0 0 -2.3586 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.7949 -7.6754 0 0 0 -2.3586 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -1 16 0 0 -0 0 + 1 1 1 + + 9.2248 10.3902 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 5.49818 14.15 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 4.2559 15.4033 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.0288 9.55797 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1 16 0 0 0 -1.5707 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 7.98256 11.6439 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.74031 12.8974 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.0288 9.55797 0.866 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 2.52922 15.9838 0 0 0 -1.5707 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 0.7646 15.9914 0 0 0 -1.5707 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -14.6681 12.3809 0 0 -0 0.810038 + 1 1 1 + + -3.55516 15.9191 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -8.84779 15.8121 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -10.6121 15.7764 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.39802 15.9276 0 0 -0 0.810038 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.6681 12.3809 0 0 0 -0.760662 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -5.3197 15.8837 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -7.08412 15.8482 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.39802 15.9276 0.866 0 -0 0.810038 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -12.2231 14.926 0 0 0 -0.760662 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.4453 13.6531 0 0 0 -0.760662 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -8.62781 9.94618 0 0 -0 0.2056 + 1 1 1 + + 2.5269 6.54195 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1.88879 9.46171 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -3.36079 10.4351 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.48387 5.89136 0 0 -0 0.205596 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -8.62781 9.94618 0 0 0 -1.3651 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 1.05488 7.51562 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -0.41711 8.4891 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.48387 5.89136 0.866 0 -0 0.205596 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -5.16961 10.6508 0 0 0 -1.3651 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -6.89862 10.298 0 0 0 -1.3651 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -16.7548 -4.64388 0 0 -0 0 + 1 1 1 + + -16.7548 -4.64388 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -17.1282 -4.13814 0 0 -0 0 + 1 1 1 + + -17.1282 -4.13814 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3218 -9.21435 0 0 -0 0 + 1 1 1 + + 10.3218 -9.21435 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3067 -12.7762 0 0 -0 0 + 1 1 1 + + 10.3067 -12.7762 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3176 -7.4365 0 0 -0 0 + 1 1 1 + + 10.3176 -7.4365 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3136 -11.0025 0 0 -0 0 + 1 1 1 + + 10.3136 -11.0025 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -16.2875 -3.11229 0 0 0 -0.509254 + 1 1 1 + + -16.2875 -3.11229 0 0 0 -0.509254 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -17.3228 -5.14571 0 0 0 -2.31969 + 1 1 1 + + -17.3228 -5.14571 0 0 0 -2.31969 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 0 0 -0 0 + 1 1 1 + + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 10 0 -0 0 + + + + + 1.03059 2.69165 18.1021 -0 1.5578 -3.12116 + orbit + perspective + + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + 10 -9.57988 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + 10.4084 -11 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + 10.4143 -13 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -1.5566 -14 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -1 -1 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.2559 -0.59667 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.49818 -1.85004 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.74031 -3.10261 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.98256 -4.3561 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.2248 -5.60978 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -1 16 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -14.5593 -16 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.2559 -0.59667 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.49818 -1.85004 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.74031 -3.10261 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.98256 -4.3561 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.2248 -5.60978 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + 25 -15 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -25 -4 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -16.4458 -4 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -17.5466 -5 0 0 -0 0 + + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -17 -4.51853 0 0 -0 0 + + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -17.425 -4 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.2559 -0.59667 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.49818 -1.85004 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.74031 -3.10261 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.98256 -4.3561 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.2248 -5.60978 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -9 10 0 0 -0 0 + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + + diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/worlds/ridgeback_race_no_lidar.world b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/worlds/ridgeback_race_no_lidar.world new file mode 100644 index 000000000..7a6a32d6e --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/worlds/ridgeback_race_no_lidar.world @@ -0,0 +1,4881 @@ + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + 0 + 0 + 0 + + + + 1 + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + + + + + + + + 10 + + + 0 + + + 0 0 1 + 100 100 + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + + + 0.001 + 1 + 1000 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + EARTH_WGS84 + 0 + 0 + 0 + 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -1 -6 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -4 1.55828 0 0 -0 0 + + + 3116 755000000 + 356 954966630 + 1605650970 323926707 + 355029 + + -2.6481 -4.01574 -0.045465 0 0 -0.74752 + 1 1 1 + + 8.54538 -6.87025 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.25439 -7.04543 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 1.49008 -7.104 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.8196 -6.78892 -0.045465 0 0 -0.74752 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.6481 -4.01574 -0.045465 0 0 -2.31822 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.78254 -6.92811 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 5.0182 -6.98712 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.8196 -6.78892 0.820535 0 0 -0.74752 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -0.070844 -6.42684 -0.045465 0 0 -2.31822 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1.3598 -5.22163 -0.045465 0 0 -2.31822 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -15.3977 -1.55916 0 0 -0 1.04571 + 1 1 1 + + -15.0829 9.98827 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.7448 4.86628 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.2984 3.15835 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -15.4433 11.2132 0 0 -0 1.04571 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -15.3977 -1.55916 0 0 0 -0.524988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.6375 8.28162 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.1908 6.57377 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -15.4433 11.2132 0.866 0 -0 1.04571 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.6146 1.4865 0 0 0 -0.524988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.5057 -0.036576 0 0 0 -0.524988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -2.60683 -10.2645 0 0 0 -0.76918 + 1 1 1 + + 8.5222 -13.3608 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.22866 -13.4213 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 1.46349 -13.4417 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.79789 -13.3071 0 0 0 -0.76918 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.60683 -10.2645 0 0 0 -2.33988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.75852 -13.3804 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 4.99332 -13.4012 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.79789 -13.3071 0.866 0 0 -0.76918 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -0.082399 -12.7309 0 0 0 -2.33988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1.34495 -11.498 0 0 0 -2.33988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3347 -5.65314 0 0 -0 1.56441 + 1 1 1 + + 4.88352 4.5315 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 8.58481 0.746571 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.81915 -0.515431 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.96327 5.4166 0 0 -0 1.56441 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.3347 -5.65314 0 0 0 -0.006293 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.11634 3.2701 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 7.35101 2.00838 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.96327 5.4166 0.866 0 -0 1.56441 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.3734 -2.1241 0 0 0 -0.006293 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.3545 -3.88863 0 0 0 -0.006293 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -16.0335 -6.41848 0 0 0 -0.7879 + 1 1 1 + + -4.96438 -9.72253 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -10.2581 -9.68398 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -12.0234 -9.67127 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -3.68791 -9.69271 0 0 0 -0.7879 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -16.0335 -6.41848 0 0 0 -2.3586 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -6.72812 -9.70918 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -8.4934 -9.69692 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -3.68791 -9.69271 0.866 0 0 -0.7879 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.5557 -8.93166 0 0 0 -2.3586 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.7949 -7.6754 0 0 0 -2.3586 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -1 16 0 0 -0 0 + 1 1 1 + + 9.2248 10.3902 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 5.49818 14.15 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 4.2559 15.4033 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.0288 9.55797 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1 16 0 0 0 -1.5707 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 7.98256 11.6439 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.74031 12.8974 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.0288 9.55797 0.866 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 2.52922 15.9838 0 0 0 -1.5707 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 0.7646 15.9914 0 0 0 -1.5707 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -14.6681 12.3809 0 0 -0 0.810038 + 1 1 1 + + -3.55516 15.9191 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -8.84779 15.8121 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -10.6121 15.7764 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.39802 15.9276 0 0 -0 0.810038 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.6681 12.3809 0 0 0 -0.760662 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -5.3197 15.8837 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -7.08412 15.8482 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.39802 15.9276 0.866 0 -0 0.810038 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -12.2231 14.926 0 0 0 -0.760662 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.4453 13.6531 0 0 0 -0.760662 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -8.62781 9.94618 0 0 -0 0.2056 + 1 1 1 + + 2.5269 6.54195 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1.88879 9.46171 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -3.36079 10.4351 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.48387 5.89136 0 0 -0 0.205596 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -8.62781 9.94618 0 0 0 -1.3651 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 1.05488 7.51562 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -0.41711 8.4891 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.48387 5.89136 0.866 0 -0 0.205596 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -5.16961 10.6508 0 0 0 -1.3651 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -6.89862 10.298 0 0 0 -1.3651 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -16.7548 -4.64388 0 0 -0 0 + 1 1 1 + + -16.7548 -4.64388 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -17.1282 -4.13814 0 0 -0 0 + 1 1 1 + + -17.1282 -4.13814 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3218 -9.21435 0 0 -0 0 + 1 1 1 + + 10.3218 -9.21435 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3067 -12.7762 0 0 -0 0 + 1 1 1 + + 10.3067 -12.7762 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3176 -7.4365 0 0 -0 0 + 1 1 1 + + 10.3176 -7.4365 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3136 -11.0025 0 0 -0 0 + 1 1 1 + + 10.3136 -11.0025 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -16.2875 -3.11229 0 0 0 -0.509254 + 1 1 1 + + -16.2875 -3.11229 0 0 0 -0.509254 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -17.3228 -5.14571 0 0 0 -2.31969 + 1 1 1 + + -17.3228 -5.14571 0 0 0 -2.31969 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 0 0 -0 0 + 1 1 1 + + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0.127058 -0.012625 0.008401 -5.7e-05 0.003353 -0.002954 + 1 1 1 + + 0.127058 -0.012625 0.008401 -5.7e-05 0.003353 -0.002954 + 0.001954 -0.000866 -0.061964 -0.008321 0.009978 -0.001386 + 0.001244 0.001765 0.002452 -0.135997 -0.01007 0.000878 + 0.001244 0.001765 0.002452 0 -0 0 + + + 0.127066 -0.012614 0.008412 -2.8e-05 0.003314 -0.002936 + 0.001162 -0.002502 -0.062977 -0.012557 0.01357 -0.00402 + -0.002239 0.004917 -0.002962 -0.159433 0.012841 0.004083 + -0.002239 0.004917 -0.002962 0 -0 0 + + + 0.127057 -0.012617 0.0084 -6.9e-05 0.003501 -0.00297 + 0.002415 -0.00193 -0.061395 -0.005008 -0.011677 0.001501 + -0.003148 0.006146 -0.003368 -0.028835 -0.016126 -0.002503 + -0.000393 0.000768 -0.000421 0 -0 0 + + + 0.127059 -0.012617 0.008403 -5.8e-05 0.003458 -0.002962 + 0.002127 -0.002056 -0.061757 -0.007075 -0.00588 0.000236 + -0.001696 0.001569 -0.002879 -0.058616 -0.009069 -0.001539 + -0.001696 0.001569 -0.002879 0 -0 0 + + + 0.196289 -0.059815 0.115167 -5.7e-05 0.003458 -0.002962 + 0.001535 -0.001423 -0.061086 -0.007193 -0.005937 0.000309 + -0.003549 0.013582 0.00361 -0.058136 -0.009259 -0.001821 + -0.000124 0.000475 0.000126 0 -0 0 + + + -0.049765 0.051908 0.005015 -0.045565 -1.47685 -1.24358 + 0.002355 -0.002164 -0.063759 1.02255 1.31025 -0 + -0.00117 0.003147 -0.007182 0 -0 0 + -1e-06 3e-06 -7e-06 0 -0 0 + + + -0.050165 -0.076075 0.004993 1.90095 0.495873 2.6548 + 0.005449 -0.004628 -0.024591 0.926043 1.08964 0 + -0.023688 0.021829 0.247248 1.35553 -0.826371 3.14159 + -2.4e-05 2.2e-05 0.000247 0 -0 0 + + + 0.060947 -0.012566 -19432.4 0 -0 0 + 0 0 -617.145 0 -0 0 + 0 0 -9.8 0 -0 0 + 0 0 -9.8 0 -0 0 + + + 0.106509 -0.01259 0.041208 3.13937 1.12267 3.13728 + 0.002393 -0.00053 -0.06195 0.008576 0.03027 0.007711 + -0.002374 0.001791 -0.006185 -0.054285 -0.072005 -0.002259 + -0.000237 0.000179 -0.000618 0 -0 0 + + + 0.106061 -0.012586 0.040984 3.13916 1.14848 3.13713 + 0.000902 -0.000269 -0.06003 0.004974 0.030614 0.00275 + -0.001844 0.001728 0.003565 -0.052403 -0.057989 -0.04024 + -0.000184 0.000173 0.000357 0 -0 0 + + + + 0 0 10 0 -0 0 + + + + + 1.03059 2.69165 18.1021 -0 1.5578 -3.12116 + orbit + perspective + + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + 10 -9.57988 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + 10.4084 -11 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + 10.4143 -13 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -1.5566 -14 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -1 -1 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.2559 -0.59667 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.49818 -1.85004 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.74031 -3.10261 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.98256 -4.3561 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.2248 -5.60978 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -1 16 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -14.5593 -16 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.2559 -0.59667 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.49818 -1.85004 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.74031 -3.10261 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.98256 -4.3561 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.2248 -5.60978 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + 25 -15 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -25 -4 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -16.4458 -4 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -17.5466 -5 0 0 -0 0 + + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -17 -4.51853 0 0 -0 0 + + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -17.425 -4 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.2559 -0.59667 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.49818 -1.85004 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.74031 -3.10261 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.98256 -4.3561 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.2248 -5.60978 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -9 10 0 0 -0 0 + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + + -2 -0.5 0.01 0 -0 0 + + 0 + + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + 1 + + 0 + 0 + + + + -0.064 0 0.048 0 -0 0 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + 1 + + + -0.064 0 0.048 0 -0 0 + + + 0.265 0.265 0.089 + + + + + + + + + + + + + + + 10 + + + -0.064 0 0 0 -0 0 + + + model://turtlebot3_waffle/meshes/waffle_base.dae + 0.001 0.001 0.001 + + + + 0 + 0 + 0 + + + + 1 + 200 + + + + + 0 + 0.0002 + + + + + 0 + 0.0002 + + + + + 0 + 0.0002 + + + + + + + 0 + 0.017 + + + + + 0 + 0.017 + + + + + 0 + 0.017 + + + + + + + ~/out:=imu + + + + 0 + + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + 1 + + 0 + 0 + + + + -0.052 0 0.111 0 -0 0 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + 0.125 + + + -0.052 0 0.111 0 -0 0 + + + 0.0508 + 0.055 + + + + + + + + + + + + + + + 10 + + + -0.064 0 0.121 0 -0 0 + + + model://turtlebot3_waffle/meshes/lds.dae + 0.001 0.001 0.001 + + + + + 1 + 0 + -0.064 0 0.121 0 -0 0 + 5 + + + + 360 + 1 + 0 + 6.28 + + + 1 + 0 + 0 + + + + 0.32 + 20.5 + 0.015 + + + gaussian + 0 + 0.01 + + + + + ~/out:=scan + + sensor_msgs/LaserScan + base_scan + + + 0 + 0 + 0 + + + + 0 0.144 0.023 -1.57 0 0 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + 0.1 + + + 0 0.144 0.023 -1.57 0 0 + + + 0.033 + 0.018 + + + + + + 100000 + 100000 + 0 0 0 + 0 + 0 + + + + + + + + 0 + 0.2 + 100000 + 1 + 0.01 + 0.001 + + + + + 10 + + + 0 0.144 0.023 0 -0 0 + + + model://turtlebot3_waffle/meshes/left_tire.dae + 0.001 0.001 0.001 + + + + 0 + 0 + 0 + + + + 0 -0.144 0.023 -1.57 0 0 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + 0.1 + + + 0 -0.144 0.023 -1.57 0 0 + + + 0.033 + 0.018 + + + + + + 100000 + 100000 + 0 0 0 + 0 + 0 + + + + + + + + 0 + 0.2 + 100000 + 1 + 0.01 + 0.001 + + + + + 10 + + + 0 -0.144 0.023 0 -0 0 + + + model://turtlebot3_waffle/meshes/right_tire.dae + 0.001 0.001 0.001 + + + + 0 + 0 + 0 + + + -0.177 -0.064 -0.004 0 -0 0 + + 0.001 + + 1e-05 + 0 + 0 + 1e-05 + 0 + 1e-05 + + 0 0 0 0 -0 0 + + + + + 0.005 + + + + + + 0 + 0.2 + 100000 + 1 + 0.01 + 0.001 + + + + + + + + + + + 10 + + 0 + 0 + 0 + + + -0.177 0.064 -0.004 0 -0 0 + + 0.001 + + 1e-05 + 0 + 0 + 1e-05 + 0 + 1e-05 + + 0 0 0 0 -0 0 + + + + + 0.005 + + + + + + 0 + 0.2 + 100000 + 1 + 0.01 + 0.001 + + + + + + + + + + + 10 + + 0 + 0 + 0 + + + + 0.069 -0.047 0.107 0 -0 0 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + 0.035 + + + 0 0.047 -0.005 0 -0 0 + + + 0.008 0.13 0.022 + + + + + + + + + + + + + + + 10 + + 0.069 -0.047 0.107 0 -0 0 + + 1 + 5 + 0.064 -0.047 0.107 0 -0 0 + + 1.047 + + 320 + 240 + + + 0.1 + 100 + + + + + intel_realsense_r200_depth + camera_depth_frame + 0.07 + 0.001 + 5.0 + + + + 0 0.047 -0.005 0 -0 0 + + + 0.008 0.13 0.022 + + + + + + + + + + + + + + + 10 + + 0.069 -0.047 0.107 0 -0 0 + 0 + 0 + 0 + + + base_footprint + base_link + 0 0 0.01 0 -0 0 + + + base_link + wheel_left_link + 0 0.144 0.023 -1.57 0 0 + + 0 0 1 + + -1e+16 + 1e+16 + + + 0 + 0 + + + + + base_link + wheel_right_link + 0 -0.144 0.023 -1.57 0 0 + + 0 0 1 + + -1e+16 + 1e+16 + + + 0 + 0 + + + + + base_link + caster_back_right_link + + + base_link + caster_back_left_link + + + base_link + base_scan + -0.064 0 0.121 0 -0 0 + + 0 0 1 + + -1e+16 + 1e+16 + + + + + base_link + camera_link + 0.064 -0.065 0.094 0 -0 0 + + 0 0 1 + + -1e+16 + 1e+16 + + + + + camera_link + camera_rgb_frame + 0.005 0.018 0.013 0 -0 0 + + 0 0 1 + + -1e+16 + 1e+16 + + + + + + 30 + wheel_left_joint + wheel_right_joint + 0.287 + 0.066 + 20 + 1.0 + cmd_vel + 1 + 1 + 0 + odom + odom + base_footprint + + + + ~/out:=joint_states + + 30 + wheel_left_joint + wheel_right_joint + + + + diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/remove_container.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/remove_container.sh index 1247aae92..e3880a51d 100755 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/remove_container.sh +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/remove_container.sh @@ -1 +1,2 @@ +sudo docker stop smacc_ue sudo docker rm smacc_ue From 9e6e88f7a873b953f1a65541c788865139193b0c Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Thu, 10 Aug 2023 19:12:35 +0200 Subject: [PATCH 50/70] minor --- .../docker/run_docker_container_bash_development.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_bash_development.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_bash_development.sh index c446c50a5..1ee8c6830 100755 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_bash_development.sh +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_bash_development.sh @@ -5,4 +5,4 @@ DIR="$(dirname "$(realpath "$0")")" ROOT_DIR=`realpath $DIR/../../../..` DRIVER=$(apt list --installed | grep nvidia-driver) -sudo nvidia-docker run --net host --ipc=host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name smacc_ue -v $ROOT_DIR/ue_project_1:/home/ros2_ws/src/ue_project_1 -v $ROOT_DIR/SMACC2:/home/ros2_ws/src/SMACC2 -v $ROOT_DIR/UE-Plugins:/home/ros2_ws/src/ue_project_1/Plugins/RapyutaSimulationPlugins -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash -c "./nvidia-driver-check.sh '$DRIVER' && bash" +sudo nvidia-docker run --net host --ipc=host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name smacc_ue -v $ROOT_DIR/ue_project_1:/home/ros2_ws/src/ue_project_1 -v $ROOT_DIR/SMACC2:/home/ros2_ws/src/SMACC2 -v $ROOT_DIR/UE-Plugins:/home/ros2_ws/src/ue_project_1/Plugins/RapyutaSimulationPlugins -v $ROOT_DIR/rclUE:/home/ros2_ws/src/ue_project_1/Plugins/rclUE -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash -c "./nvidia-driver-check.sh '$DRIVER' && bash" From 4c5ae0e6c89e49871dc6c1411df134f3a4dc7235 Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Thu, 10 Aug 2023 20:39:40 +0200 Subject: [PATCH 51/70] push --- .../cp_waypoints_navigator.cpp | 7 + .../cb_load_waypoints_file.hpp | 9 + .../modestates/ms_dance_bot_recovery_mode.hpp | 2 +- .../modestates/ms_dance_bot_run_mode.hpp | 2 +- .../include/sm_dancebot_ue/sm_dancebot_ue.hpp | 2 +- .../launch/sm_dancebot_ue_launch.py | 2 +- .../nav2z_client_husky/nav2_params.yaml | 367 ----------------- .../nav2z_client_husky/navigation_tree.xml | 27 -- .../nav2z_client_husky/waypoints_plan.yaml | 82 ---- .../nav2z_client_turtlebot/nav2_params.yaml | 369 ------------------ .../navigation_tree.xml | 27 -- .../waypoints_plan.yaml | 82 ---- .../params/sm_dancebot_ue_config.yaml | 2 +- .../src/sm_dancebot_ue/sm_dancebot_ue.cpp | 2 +- 14 files changed, 22 insertions(+), 960 deletions(-) delete mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_husky/nav2_params.yaml delete mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_husky/navigation_tree.xml delete mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_husky/waypoints_plan.yaml delete mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_turtlebot/nav2_params.yaml delete mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_turtlebot/navigation_tree.xml delete mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_turtlebot/waypoints_plan.yaml diff --git a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.cpp b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.cpp index d3163961a..ef31e3323 100644 --- a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.cpp +++ b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.cpp @@ -159,7 +159,14 @@ void CpWaypointNavigatorBase::loadWaypointsFromYamlParameter( { std::string package_share_directory = ament_index_cpp::get_package_share_directory(yaml_file_package_name); + + RCLCPP_INFO(getLogger(), "file macro path: %s", planfilepath.c_str()); + boost::replace_all(planfilepath, "$(pkg_share)", package_share_directory); + + RCLCPP_INFO(getLogger(), "package share path: %s", package_share_directory.c_str()); + RCLCPP_INFO(getLogger(), "waypoints plan file: %s", planfilepath.c_str()); + this->loadWayPointsFromFile(planfilepath); RCLCPP_INFO(getLogger(), "waypoints plan: %s", planfilepath.c_str()); } diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_load_waypoints_file.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_load_waypoints_file.hpp index 412703ad3..5da38588f 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_load_waypoints_file.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_load_waypoints_file.hpp @@ -42,10 +42,19 @@ struct CbLoadWaypointsFile : public smacc2::SmaccClientBehavior requiresComponent(waypointsNavigator_); // this is a component from the nav2z_client library if (filepath_) + { + RCLCPP_INFO_STREAM( + getNode()->get_logger(), "[CbLoadWaypointsFile] Loading waypoints from file: " << *filepath_); this->waypointsNavigator_->loadWayPointsFromFile(filepath_.value()); + } else + { + RCLCPP_INFO_STREAM( + getNode()->get_logger(), "[CbLoadWaypointsFile] Loading waypoints from parameter: " + << parameterName_.value() << " in package: " << packagenamesapce_.value()); this->waypointsNavigator_->loadWaypointsFromYamlParameter( parameterName_.value(), packagenamesapce_.value()); + } // change this to skip some points of the yaml file, default = 0 waypointsNavigator_->currentWaypoint_ = 0; diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/modestates/ms_dance_bot_recovery_mode.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/modestates/ms_dance_bot_recovery_mode.hpp index 18ac2c5f3..fc56219e3 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/modestates/ms_dance_bot_recovery_mode.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/modestates/ms_dance_bot_recovery_mode.hpp @@ -22,7 +22,7 @@ namespace sm_dancebot_ue { // STATE DECLARATION -class MsDanceBotRecoveryMode : public smacc2::SmaccState +class MsDanceBotRecoveryMode : public smacc2::SmaccState { public: using SmaccState::SmaccState; diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/modestates/ms_dance_bot_run_mode.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/modestates/ms_dance_bot_run_mode.hpp index a8a9b44ec..d220c9f74 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/modestates/ms_dance_bot_run_mode.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/modestates/ms_dance_bot_run_mode.hpp @@ -22,7 +22,7 @@ namespace sm_dancebot_ue { // STATE DECLARATION -class MsDanceBotRunMode : public smacc2::SmaccState +class MsDanceBotRunMode : public smacc2::SmaccState { public: using SmaccState::SmaccState; diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/sm_dancebot_ue.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/sm_dancebot_ue.hpp index 2f057c941..1644ab4c1 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/sm_dancebot_ue.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/sm_dancebot_ue.hpp @@ -110,7 +110,7 @@ namespace sm_dancebot_ue { /// \brief Advanced example of state machine with smacc that shows multiple techniques /// for the development of state machines -struct SmDanceBot : public smacc2::SmaccStateMachineBase +struct SmDanceBotUE : public smacc2::SmaccStateMachineBase { typedef mpl::bool_ shallow_history; typedef mpl::bool_ deep_history; diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/launch/sm_dancebot_ue_launch.py b/smacc2_sm_reference_library/sm_dancebot_ue/launch/sm_dancebot_ue_launch.py index 6ea966123..3ac471f47 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/launch/sm_dancebot_ue_launch.py +++ b/smacc2_sm_reference_library/sm_dancebot_ue/launch/sm_dancebot_ue_launch.py @@ -190,7 +190,7 @@ def generate_launch_description(): sm_dance_bot_node = Node( package="sm_dancebot_ue", executable="sm_dancebot_ue_node", - name="SmDanceBot", + name="SmDanceBotUE", output="screen", prefix=xtermprefix, parameters=[ diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_husky/nav2_params.yaml b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_husky/nav2_params.yaml deleted file mode 100644 index 08e344730..000000000 --- a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_husky/nav2_params.yaml +++ /dev/null @@ -1,367 +0,0 @@ -amcl: - ros__parameters: - use_sim_time: True - alpha1: 0.2 - alpha2: 0.2 - alpha3: 0.2 - alpha4: 0.2 - alpha5: 0.2 - base_frame_id: "base_footprint" - beam_skip_distance: 0.5 - beam_skip_error_threshold: 0.9 - beam_skip_threshold: 0.3 - do_beamskip: false - global_frame_id: "map" - lambda_short: 0.1 - laser_likelihood_max_dist: 2.0 - laser_max_range: 100.0 - laser_min_range: -1.0 - laser_model_type: "likelihood_field" - max_beams: 60 - max_particles: 2000 - min_particles: 500 - odom_frame_id: "odom" - pf_err: 0.05 - pf_z: 0.99 - recovery_alpha_fast: 0.0 - recovery_alpha_slow: 0.0 - resample_interval: 1 - robot_model_type: "differential" - save_pose_rate: 0.5 - sigma_hit: 0.2 - tf_broadcast: true - transform_tolerance: 1.0 - update_min_a: 0.2 - update_min_d: 0.25 - z_hit: 0.5 - z_max: 0.05 - z_rand: 0.5 - z_short: 0.05 - scan_topic: scan - -amcl_map_client: - ros__parameters: - use_sim_time: True - -amcl_rclcpp_node: - ros__parameters: - use_sim_time: True - -bt_navigator: - ros__parameters: - use_sim_time: True - global_frame: map - robot_base_frame: base_link - odom_topic: /odom - enable_groot_monitoring: True - groot_zmq_publisher_port: 1666 - groot_zmq_server_port: 1667 - default_nav_to_pose_bt_xml: RUNTIMEFILL - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_back_up_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - -bt_navigator_rclcpp_node: - ros__parameters: - use_sim_time: True - -controller_server: - ros__parameters: - use_sim_time: True - controller_frequency: 20.0 - min_x_velocity_threshold: 0.001 - min_y_velocity_threshold: 0.5 - min_theta_velocity_threshold: 0.001 - progress_checker_plugin: "progress_checker" - goal_checker_plugins: ["goal_checker", "forward_goal_checker", "backward_goal_checker", "absolute_rotate_goal_checker", "undo_path_backwards_goal_checker"] - controller_plugins: ["FollowPath","BackwardLocalPlanner", "ForwardLocalPlanner", "PureSpinningLocalPlanner"] - - # Progress checker parameters - progress_checker: - plugin: "nav2_controller::SimpleProgressChecker" - required_movement_radius: 0.001 - movement_time_allowance: 3000.0 - - # Goal checker parameters - goal_checker: - plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 0.257 - yaw_goal_tolerance: 0.165 # almost free orientation - stateful: True - backward_goal_checker: - plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 0.09 - yaw_goal_tolerance: 0.05 - stateful: True - forward_goal_checker: - plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 0.09 - yaw_goal_tolerance: 0.05 - stateful: True - absolute_rotate_goal_checker: - plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 0.15 - yaw_goal_tolerance: 0.01 #0.005 - stateful: True - undo_path_backwards_goal_checker: - plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 0.03 # 4 cm - yaw_goal_tolerance: 0.1 - stateful: True - - # DWB parameters - FollowPath: - plugin: "dwb_core::DWBLocalPlanner" - debug_trajectory_details: True - min_vel_x: 0.0 - min_vel_y: 0.0 - max_vel_x: 0.3 - max_vel_y: 0.0 - max_vel_theta: 1.0 - min_speed_xy: 0.0 - max_speed_xy: 0.05 - min_speed_theta: 0.0 - # Add high threshold velocity for turtlebot 3 issue. - # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 - acc_lim_x: 2.5 - acc_lim_y: 0.0 - acc_lim_theta: 3.2 - decel_lim_x: -2.5 - decel_lim_y: 0.0 - decel_lim_theta: -3.2 - vx_samples: 20 - vy_samples: 5 - vtheta_samples: 20 - sim_time: 1.7 - linear_granularity: 0.05 - angular_granularity: 0.025 - transform_tolerance: 0.2 - xy_goal_tolerance: 0.157 - yaw_goal_tolerance: 0.15 - trans_stopped_velocity: 0.25 - short_circuit_trajectory_evaluation: True - stateful: True - critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] - BaseObstacle.scale: 0.02 - PathAlign.scale: 32.0 - PathAlign.forward_point_distance: 0.1 - GoalAlign.scale: 24.0 - GoalAlign.forward_point_distance: 0.1 - PathDist.scale: 32.0 - GoalDist.scale: 24.0 - RotateToGoal.scale: 32.0 - RotateToGoal.slowing_factor: 5.0 - RotateToGoal.lookahead_time: -1.0 - BackwardLocalPlanner: - plugin: "backward_local_planner::BackwardLocalPlanner" - transform_tolerance: 0.5 - k_rho: -1.4 # linear distance to the goal gain, default = -1.0 (related with carrot distance) - k_alpha: -0.1 #-0.1 - k_betta: -0.1 # -1.0 - carrot_distance: 0.3 #meters default 0.5 - carrot_angular_distance: 0.3 # no constraint, max 3.1416 - pure_spinning_straight_line_mode: true - max_linear_x_speed: 0.25 - ForwardLocalPlanner: - plugin: "cl_nav2z::forward_local_planner::ForwardLocalPlanner" - transform_tolerance: 0.5 - k_rho: 2.5 - k_alpha: -0.4 - k_betta: -1.0 - carrot_distance: 0.2 #meters - carrot_angular_distance: 0.5 # no constraint, max 3.1416 - max_linear_x_speed: 0.3 - max_angular_z_speed: 0.2 - PureSpinningLocalPlanner: - plugin: "pure_spinning_local_planner::PureSpinningLocalPlanner" - transform_tolerance: 0.5 - k_betta: 10.0 - max_angular_z_speed: 0.2 - use_shortest_angular_distance: true - - -controller_server_rclcpp_node: - ros__parameters: - use_sim_time: True - -local_costmap: - local_costmap: - ros__parameters: - update_frequency: 5.0 - publish_frequency: 2.0 - global_frame: odom - robot_base_frame: base_link - use_sim_time: True - rolling_window: true - width: 3 - height: 3 - resolution: 0.05 - robot_radius: 0.22 - plugins: ["voxel_layer", "inflation_layer"] - inflation_layer: - plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 3.0 - inflation_radius: 0.55 - voxel_layer: - plugin: "nav2_costmap_2d::VoxelLayer" - enabled: True - publish_voxel_map: True - origin_z: 0.0 - z_resolution: 0.05 - z_voxels: 16 - max_obstacle_height: 2.0 - mark_threshold: 0 - observation_sources: scan - scan: - topic: /scan - max_obstacle_height: 2.0 - clearing: True - marking: True - data_type: "LaserScan" - raytrace_max_range: 15.0 - raytrace_min_range: 0.0 - obstacle_max_range: 15.5 - obstacle_min_range: 0.0 - static_layer: - map_subscribe_transient_local: True - always_send_full_costmap: True - local_costmap_client: - ros__parameters: - use_sim_time: True - local_costmap_rclcpp_node: - ros__parameters: - use_sim_time: True - -global_costmap: - global_costmap: - ros__parameters: - update_frequency: 1.0 - publish_frequency: 1.0 - transform_tolerance: 5.0 - global_frame: map - robot_base_frame: base_link - use_sim_time: True - robot_radius: 0.22 - resolution: 0.05 - track_unknown_space: true - plugins: ["static_layer", "obstacle_layer", "inflation_layer"] - obstacle_layer: - plugin: "nav2_costmap_2d::ObstacleLayer" - enabled: True - observation_sources: scan - scan: - topic: /scan - max_obstacle_height: 2.0 - clearing: True - marking: True - data_type: "LaserScan" - raytrace_max_range: 15.0 - raytrace_min_range: 0.0 - obstacle_max_range: 15.5 - obstacle_min_range: 0.0 - static_layer: - plugin: "nav2_costmap_2d::StaticLayer" - map_subscribe_transient_local: True - inflation_layer: - plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 3.0 - inflation_radius: 0.55 - always_send_full_costmap: True - global_costmap_client: - ros__parameters: - use_sim_time: True - global_costmap_rclcpp_node: - ros__parameters: - use_sim_time: True - -map_server: - ros__parameters: - use_sim_time: True - yaml_filename: "turtlebot3_world.yaml" - -map_saver: - ros__parameters: - use_sim_time: True - save_map_timeout: 5000 - free_thresh_default: 0.25 - occupied_thresh_default: 0.65 - -planner_server: - ros__parameters: - expected_planner_frequency: 20.0 - use_sim_time: True - planner_plugins: ["GridBased", "ForwardGlobalPlanner", "BackwardGlobalPlanner", "UndoPathGlobalPlanner"] - GridBased: - plugin: "nav2_navfn_planner/NavfnPlanner" - tolerance: 10.0 #meters - use_astar: false - allow_unknown: true - ForwardGlobalPlanner: - plugin: "forward_global_planner::ForwardGlobalPlanner" - transform_tolerance: 0.5 - BackwardGlobalPlanner: - plugin: "backward_global_planner::BackwardGlobalPlanner" - transform_tolerance: 0.5 - UndoPathGlobalPlanner: - plugin: "undo_path_global_planner::UndoPathGlobalPlanner" - transform_tolerance: 0.5 - -planner_server_rclcpp_node: - ros__parameters: - use_sim_time: True - -recoveries_server: - ros__parameters: - costmap_topic: local_costmap/costmap_raw - footprint_topic: local_costmap/published_footprint - cycle_frequency: 10.0 - recovery_plugins: ["spin", "backup", "wait"] - #recovery_plugins: ["spin"] - spin: - plugin: "nav2_recoveries/Spin" - backup: - plugin: "nav2_recoveries/BackUp" - wait: - plugin: "nav2_recoveries/Wait" - global_frame: odom - robot_base_frame: base_link - transform_timeout: 0.1 - use_sim_time: true - simulate_ahead_time: 2.0 - max_rotational_vel: 1.0 - min_rotational_vel: 0.4 - rotational_acc_lim: 3.2 - -robot_state_publisher: - ros__parameters: - use_sim_time: True diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_husky/navigation_tree.xml b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_husky/navigation_tree.xml deleted file mode 100644 index e599e5d4f..000000000 --- a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_husky/navigation_tree.xml +++ /dev/null @@ -1,27 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_husky/waypoints_plan.yaml b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_husky/waypoints_plan.yaml deleted file mode 100644 index 64adbe1b0..000000000 --- a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_husky/waypoints_plan.yaml +++ /dev/null @@ -1,82 +0,0 @@ -waypoints: - # POINT 0 - - position: - x: 3.0 - y: -2.0 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.0 - w: 1.0 - # POINT 1 - - position: - x: -5.75 - y: 3.5 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.0 - w: 1.0 - # POINT 2 - - position: - x: -10.25 - y: -4.25 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.0 - w: 1.0 - # POINT 3 - - position: - x: 2.0 - y: -8.58 # north west corner of the room - #y: -12.35 # south west corner of the room - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.0 - w: 1.0 - # POINT 4 - - position: - x: -12.00 - y: 12.0 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.0 - w: 1.0 - # POINT 5 - - position: - x: -10.0 - y: 14.5 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.0 - w: 1.0 - # POINT 6 - - position: - x: -0.0 - y: 0.0 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.0 - w: 1.0 - #POINT 7 - - position: - x: 6.0 - y: 8.0 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.0 - w: 1.0 diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_turtlebot/nav2_params.yaml b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_turtlebot/nav2_params.yaml deleted file mode 100644 index 57d8d63ad..000000000 --- a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_turtlebot/nav2_params.yaml +++ /dev/null @@ -1,369 +0,0 @@ -amcl: - ros__parameters: - use_sim_time: True - alpha1: 0.2 - alpha2: 0.2 - alpha3: 0.2 - alpha4: 0.2 - alpha5: 0.2 - base_frame_id: "base_footprint" - beam_skip_distance: 0.5 - beam_skip_error_threshold: 0.9 - beam_skip_threshold: 0.3 - do_beamskip: false - global_frame_id: "map" - lambda_short: 0.1 - laser_likelihood_max_dist: 2.0 - laser_max_range: 100.0 - laser_min_range: -1.0 - laser_model_type: "likelihood_field" - max_beams: 60 - max_particles: 2000 - min_particles: 500 - odom_frame_id: "odom" - pf_err: 0.05 - pf_z: 0.99 - recovery_alpha_fast: 0.0 - recovery_alpha_slow: 0.0 - resample_interval: 1 - robot_model_type: "differential" - save_pose_rate: 0.5 - sigma_hit: 0.2 - tf_broadcast: true - transform_tolerance: 1.0 - update_min_a: 0.2 - update_min_d: 0.25 - z_hit: 0.5 - z_max: 0.05 - z_rand: 0.5 - z_short: 0.05 - scan_topic: scan - -amcl_map_client: - ros__parameters: - use_sim_time: True - -amcl_rclcpp_node: - ros__parameters: - use_sim_time: True - -bt_navigator: - ros__parameters: - use_sim_time: True - global_frame: map - robot_base_frame: base_link - odom_topic: /odom - enable_groot_monitoring: True - groot_zmq_publisher_port: 1666 - groot_zmq_server_port: 1667 - default_nav_to_pose_bt_xml: RUNTIMEFILL - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_back_up_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - -bt_navigator_rclcpp_node: - ros__parameters: - use_sim_time: True - -controller_server: - ros__parameters: - use_sim_time: True - controller_frequency: 20.0 - min_x_velocity_threshold: 0.001 - min_y_velocity_threshold: 0.5 - min_theta_velocity_threshold: 0.001 - progress_checker_plugin: "progress_checker" - goal_checker_plugins: ["goal_checker", "forward_goal_checker", "backward_goal_checker", "absolute_rotate_goal_checker", "undo_path_backwards_goal_checker"] - controller_plugins: ["FollowPath","BackwardLocalPlanner", "ForwardLocalPlanner", "PureSpinningLocalPlanner"] - - # Progress checker parameters - progress_checker: - plugin: "nav2_controller::SimpleProgressChecker" - required_movement_radius: 0.001 - movement_time_allowance: 3000.0 - - # Goal checker parameters - goal_checker: - plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 0.257 - yaw_goal_tolerance: 0.165 # almost free orientation - stateful: True - backward_goal_checker: - plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 0.09 - yaw_goal_tolerance: 0.05 - stateful: True - forward_goal_checker: - plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 0.09 - yaw_goal_tolerance: 0.05 - stateful: True - absolute_rotate_goal_checker: - plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 0.15 - yaw_goal_tolerance: 0.01 #0.005 - stateful: True - undo_path_backwards_goal_checker: - plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 0.03 # 4 cm - yaw_goal_tolerance: 0.1 - stateful: True - - # DWB parameters - FollowPath: - plugin: "dwb_core::DWBLocalPlanner" - debug_trajectory_details: True - min_vel_x: 0.0 - min_vel_y: 0.0 - max_vel_x: 0.3 - max_vel_y: 0.0 - max_vel_theta: 1.0 - min_speed_xy: 0.0 - max_speed_xy: 0.3 - min_speed_theta: 0.0 - # Add high threshold velocity for turtlebot 3 issue. - # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 - acc_lim_x: 2.5 - acc_lim_y: 0.0 - acc_lim_theta: 3.2 - decel_lim_x: -2.5 - decel_lim_y: 0.0 - decel_lim_theta: -3.2 - vx_samples: 20 - vy_samples: 5 - vtheta_samples: 20 - sim_time: 1.7 - linear_granularity: 0.05 - angular_granularity: 0.025 - transform_tolerance: 0.2 - xy_goal_tolerance: 0.157 - yaw_goal_tolerance: 0.15 - trans_stopped_velocity: 0.25 - short_circuit_trajectory_evaluation: True - stateful: True - critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] - BaseObstacle.scale: 0.02 - PathAlign.scale: 32.0 - PathAlign.forward_point_distance: 0.1 - GoalAlign.scale: 24.0 - GoalAlign.forward_point_distance: 0.1 - PathDist.scale: 32.0 - GoalDist.scale: 24.0 - RotateToGoal.scale: 32.0 - RotateToGoal.slowing_factor: 5.0 - RotateToGoal.lookahead_time: -1.0 - BackwardLocalPlanner: - plugin: "backward_local_planner::BackwardLocalPlanner" - transform_tolerance: 0.5 - k_rho: -1.4 # linear distance to the goal gain, default = -1.0 (related with carrot distance) - k_alpha: -0.1 #-0.1 - k_betta: -0.1 # -1.0 - carrot_distance: 0.3 #meters default 0.5 - carrot_angular_distance: 0.3 # no constraint, max 3.1416 - pure_spinning_straight_line_mode: true - max_linear_x_speed: 0.25 - ForwardLocalPlanner: - plugin: "cl_nav2z::forward_local_planner::ForwardLocalPlanner" - transform_tolerance: 0.5 - k_rho: 2.5 - k_alpha: -0.4 - k_betta: -1.0 - carrot_distance: 0.2 #meters - carrot_angular_distance: 0.5 # no constraint, max 3.1416 - max_linear_x_speed: 0.3 - max_angular_z_speed: 0.2 - PureSpinningLocalPlanner: - plugin: "pure_spinning_local_planner::PureSpinningLocalPlanner" - transform_tolerance: 0.5 - k_betta: 10.0 - max_angular_z_speed: 0.2 - use_shortest_angular_distance: true - - -controller_server_rclcpp_node: - ros__parameters: - use_sim_time: True - -local_costmap: - local_costmap: - ros__parameters: - update_frequency: 5.0 - publish_frequency: 2.0 - global_frame: odom - robot_base_frame: base_link - use_sim_time: True - rolling_window: true - width: 6 - height: 6 - resolution: 0.05 - robot_radius: 0.22 - plugins: ["voxel_layer", "inflation_layer"] - inflation_layer: - plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 3.0 - inflation_radius: 0.55 - voxel_layer: - plugin: "nav2_costmap_2d::VoxelLayer" - enabled: True - publish_voxel_map: True - origin_z: 0.0 - z_resolution: 0.05 - z_voxels: 16 - max_obstacle_height: 2.0 - mark_threshold: 0 - observation_sources: scan - scan: - topic: /scan - max_obstacle_height: 2.0 - clearing: True - marking: True - data_type: "LaserScan" - raytrace_max_range: 15.0 - raytrace_min_range: 0.0 - obstacle_max_range: 15.5 - obstacle_min_range: 0.0 - static_layer: - map_subscribe_transient_local: True - always_send_full_costmap: True - local_costmap_client: - ros__parameters: - use_sim_time: True - local_costmap_rclcpp_node: - ros__parameters: - use_sim_time: True - -global_costmap: - global_costmap: - ros__parameters: - width: 8 - height: 8 - update_frequency: 1.0 - publish_frequency: 1.0 - transform_tolerance: 5.0 - global_frame: map - robot_base_frame: base_link - use_sim_time: True - robot_radius: 0.22 - resolution: 0.05 - track_unknown_space: true - plugins: ["static_layer", "obstacle_layer", "inflation_layer"] - obstacle_layer: - plugin: "nav2_costmap_2d::ObstacleLayer" - enabled: True - observation_sources: scan - scan: - topic: /scan - max_obstacle_height: 2.0 - clearing: True - marking: True - data_type: "LaserScan" - raytrace_max_range: 15.0 - raytrace_min_range: 0.0 - obstacle_max_range: 15.5 - obstacle_min_range: 0.0 - static_layer: - plugin: "nav2_costmap_2d::StaticLayer" - map_subscribe_transient_local: True - inflation_layer: - plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 3.0 - inflation_radius: 0.55 - always_send_full_costmap: True - global_costmap_client: - ros__parameters: - use_sim_time: True - global_costmap_rclcpp_node: - ros__parameters: - use_sim_time: True - -map_server: - ros__parameters: - use_sim_time: True - yaml_filename: "turtlebot3_world.yaml" - -map_saver: - ros__parameters: - use_sim_time: True - save_map_timeout: 5000 - free_thresh_default: 0.25 - occupied_thresh_default: 0.65 - -planner_server: - ros__parameters: - expected_planner_frequency: 20.0 - use_sim_time: True - planner_plugins: ["GridBased", "ForwardGlobalPlanner", "BackwardGlobalPlanner", "UndoPathGlobalPlanner"] - GridBased: - plugin: "nav2_navfn_planner/NavfnPlanner" - tolerance: 10.0 #meters - use_astar: false - allow_unknown: true - ForwardGlobalPlanner: - plugin: "forward_global_planner::ForwardGlobalPlanner" - transform_tolerance: 0.5 - BackwardGlobalPlanner: - plugin: "backward_global_planner::BackwardGlobalPlanner" - transform_tolerance: 0.5 - UndoPathGlobalPlanner: - plugin: "undo_path_global_planner::UndoPathGlobalPlanner" - transform_tolerance: 0.5 - -planner_server_rclcpp_node: - ros__parameters: - use_sim_time: True - -recoveries_server: - ros__parameters: - costmap_topic: local_costmap/costmap_raw - footprint_topic: local_costmap/published_footprint - cycle_frequency: 10.0 - recovery_plugins: ["spin", "backup", "wait"] - #recovery_plugins: ["spin"] - spin: - plugin: "nav2_recoveries/Spin" - backup: - plugin: "nav2_recoveries/BackUp" - wait: - plugin: "nav2_recoveries/Wait" - global_frame: odom - robot_base_frame: base_link - transform_timeout: 0.1 - use_sim_time: true - simulate_ahead_time: 2.0 - max_rotational_vel: 1.0 - min_rotational_vel: 0.4 - rotational_acc_lim: 3.2 - -robot_state_publisher: - ros__parameters: - use_sim_time: True diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_turtlebot/navigation_tree.xml b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_turtlebot/navigation_tree.xml deleted file mode 100644 index e599e5d4f..000000000 --- a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_turtlebot/navigation_tree.xml +++ /dev/null @@ -1,27 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_turtlebot/waypoints_plan.yaml b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_turtlebot/waypoints_plan.yaml deleted file mode 100644 index 64adbe1b0..000000000 --- a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client_turtlebot/waypoints_plan.yaml +++ /dev/null @@ -1,82 +0,0 @@ -waypoints: - # POINT 0 - - position: - x: 3.0 - y: -2.0 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.0 - w: 1.0 - # POINT 1 - - position: - x: -5.75 - y: 3.5 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.0 - w: 1.0 - # POINT 2 - - position: - x: -10.25 - y: -4.25 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.0 - w: 1.0 - # POINT 3 - - position: - x: 2.0 - y: -8.58 # north west corner of the room - #y: -12.35 # south west corner of the room - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.0 - w: 1.0 - # POINT 4 - - position: - x: -12.00 - y: 12.0 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.0 - w: 1.0 - # POINT 5 - - position: - x: -10.0 - y: 14.5 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.0 - w: 1.0 - # POINT 6 - - position: - x: -0.0 - y: 0.0 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.0 - w: 1.0 - #POINT 7 - - position: - x: 6.0 - y: 8.0 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.0 - w: 1.0 diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/params/sm_dancebot_ue_config.yaml b/smacc2_sm_reference_library/sm_dancebot_ue/params/sm_dancebot_ue_config.yaml index f53863a47..2add60407 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/params/sm_dancebot_ue_config.yaml +++ b/smacc2_sm_reference_library/sm_dancebot_ue/params/sm_dancebot_ue_config.yaml @@ -1,4 +1,4 @@ -SmDanceBot: +SmDanceBotUE: ros__parameters: use_sim_time: true signal_detector_loop_freq: 20.0 diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/sm_dancebot_ue.cpp b/smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/sm_dancebot_ue.cpp index 7794ed0ce..2ab20a261 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/sm_dancebot_ue.cpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/sm_dancebot_ue.cpp @@ -23,5 +23,5 @@ int main(int argc, char ** argv) { rclcpp::init(argc, argv); - smacc2::run(); + smacc2::run(); } From 62d744e98b0a2e12770eb7335a183e6d8834564f Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Fri, 11 Aug 2023 12:01:30 +0200 Subject: [PATCH 52/70] waypoints fix --- .../nav2z_client/waypoints_art_gallery.yaml | 56 +-- .../nav2z_client_husky/nav2_params.yaml | 367 ----------------- .../nav2z_client_husky/navigation_tree.xml | 27 -- .../nav2z_client_husky/waypoints_plan.yaml | 82 ---- .../nav2z_client_turtlebot/nav2_params.yaml | 369 ------------------ .../navigation_tree.xml | 27 -- .../waypoints_plan.yaml | 82 ---- 7 files changed, 28 insertions(+), 982 deletions(-) delete mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client_husky/nav2_params.yaml delete mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client_husky/navigation_tree.xml delete mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client_husky/waypoints_plan.yaml delete mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client_turtlebot/nav2_params.yaml delete mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client_turtlebot/navigation_tree.xml delete mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client_turtlebot/waypoints_plan.yaml diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/waypoints_art_gallery.yaml b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/waypoints_art_gallery.yaml index 7d0de55a9..d2ede7bf2 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/waypoints_art_gallery.yaml +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/waypoints_art_gallery.yaml @@ -2,140 +2,140 @@ waypoints: # POINT R1 - position: x: 18.10907715 - y: -52.59328613 + y: 52.59328613 z: 0.0 orientation: x: 0.0 - y: 0.0 + y: -0.0 z: 0.0 w: 1.0 # POINT R2 - position: x: -21.0 - y: -40.0 + y: 40.0 z: 0.0 orientation: x: 0.0 - y: 0.0 + y: -0.0 z: 0.0 w: 1.0 # POINT R3 - position: x: -21.0 - y: -63.0 + y: 63.0 z: 0.0 orientation: x: 0.0 - y: 0.0 + y: -0.0 z: 0.0 w: 1.0 # POINT R4 - position: x: -2.0 - y: -67.0 + y: 67.0 z: 0.0 orientation: x: 0.0 - y: 0.0 + y: -0.0 z: 0.0 w: 1.0 # POINT R5 - position: x: -10.5 - y: -63.0 + y: 63.0 z: 0.0 orientation: x: 0.0 - y: 0.0 + y: -0.0 z: 0.0 w: 1.0 # POINT R7 - position: x: 2.0 - y: -35.0 + y: 35.0 z: 0.0 orientation: x: 0.0 - y: 0.0 + y: -0.0 z: 0.0 w: 1.0 # POINT R8 - position: x: 3.0 - y: -46.0 + y: 46.0 z: 0.0 orientation: x: 0.0 - y: 0.0 + y: -0.0 z: 0.0 w: 1.0 # POINT R10 - position: x: -30.88127197 - y: -53.87810547 + y: 53.87810547 z: 0.0 orientation: x: 0.0 - y: 0.0 + y: -0.0 z: 0.0 w: 1.0 # POINT R11 - position: x: -16.21995605 - y: -35.0 + y: 35.0 z: 0.0 orientation: x: 0.0 - y: 0.0 + y: -0.0 z: 0.0 w: 1.0 # POINT R12 - position: x: -28.99974121 - y: -66.22507324 + y: 66.22507324 z: 0.0 orientation: x: 0.0 - y: 0.0 + y: -0.0 z: 0.0 w: 1.0 # POINT R13 - position: x: -3.86228577 - y: -52.00396973 + y: 52.00396973 z: 0.0 orientation: x: 0.0 - y: 0.0 + y: -0.0 z: 0.0 w: 1.0 # POINT R14 - position: x: -38.07478027 - y: -63.84986816 + y: 63.84986816 z: 0.0 orientation: x: 0.0 - y: 0.0 + y: -0.0 z: 0.0 w: 1.0 # POINT R15 - position: x: -38.07478027 - y: -48.30913086 + y: 48.30913086 z: 0.0 orientation: x: 0.0 - y: 0.0 + y: -0.0 z: 0.0 w: 1.0 # POINT R16 - position: x: -7.86228577 - y: -41.00396973 + y: 41.00396973 z: 0.0 orientation: x: 0.0 - y: 0.0 + y: -0.0 z: 0.0 w: 1.0 diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client_husky/nav2_params.yaml b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client_husky/nav2_params.yaml deleted file mode 100644 index 08e344730..000000000 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client_husky/nav2_params.yaml +++ /dev/null @@ -1,367 +0,0 @@ -amcl: - ros__parameters: - use_sim_time: True - alpha1: 0.2 - alpha2: 0.2 - alpha3: 0.2 - alpha4: 0.2 - alpha5: 0.2 - base_frame_id: "base_footprint" - beam_skip_distance: 0.5 - beam_skip_error_threshold: 0.9 - beam_skip_threshold: 0.3 - do_beamskip: false - global_frame_id: "map" - lambda_short: 0.1 - laser_likelihood_max_dist: 2.0 - laser_max_range: 100.0 - laser_min_range: -1.0 - laser_model_type: "likelihood_field" - max_beams: 60 - max_particles: 2000 - min_particles: 500 - odom_frame_id: "odom" - pf_err: 0.05 - pf_z: 0.99 - recovery_alpha_fast: 0.0 - recovery_alpha_slow: 0.0 - resample_interval: 1 - robot_model_type: "differential" - save_pose_rate: 0.5 - sigma_hit: 0.2 - tf_broadcast: true - transform_tolerance: 1.0 - update_min_a: 0.2 - update_min_d: 0.25 - z_hit: 0.5 - z_max: 0.05 - z_rand: 0.5 - z_short: 0.05 - scan_topic: scan - -amcl_map_client: - ros__parameters: - use_sim_time: True - -amcl_rclcpp_node: - ros__parameters: - use_sim_time: True - -bt_navigator: - ros__parameters: - use_sim_time: True - global_frame: map - robot_base_frame: base_link - odom_topic: /odom - enable_groot_monitoring: True - groot_zmq_publisher_port: 1666 - groot_zmq_server_port: 1667 - default_nav_to_pose_bt_xml: RUNTIMEFILL - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_back_up_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - -bt_navigator_rclcpp_node: - ros__parameters: - use_sim_time: True - -controller_server: - ros__parameters: - use_sim_time: True - controller_frequency: 20.0 - min_x_velocity_threshold: 0.001 - min_y_velocity_threshold: 0.5 - min_theta_velocity_threshold: 0.001 - progress_checker_plugin: "progress_checker" - goal_checker_plugins: ["goal_checker", "forward_goal_checker", "backward_goal_checker", "absolute_rotate_goal_checker", "undo_path_backwards_goal_checker"] - controller_plugins: ["FollowPath","BackwardLocalPlanner", "ForwardLocalPlanner", "PureSpinningLocalPlanner"] - - # Progress checker parameters - progress_checker: - plugin: "nav2_controller::SimpleProgressChecker" - required_movement_radius: 0.001 - movement_time_allowance: 3000.0 - - # Goal checker parameters - goal_checker: - plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 0.257 - yaw_goal_tolerance: 0.165 # almost free orientation - stateful: True - backward_goal_checker: - plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 0.09 - yaw_goal_tolerance: 0.05 - stateful: True - forward_goal_checker: - plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 0.09 - yaw_goal_tolerance: 0.05 - stateful: True - absolute_rotate_goal_checker: - plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 0.15 - yaw_goal_tolerance: 0.01 #0.005 - stateful: True - undo_path_backwards_goal_checker: - plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 0.03 # 4 cm - yaw_goal_tolerance: 0.1 - stateful: True - - # DWB parameters - FollowPath: - plugin: "dwb_core::DWBLocalPlanner" - debug_trajectory_details: True - min_vel_x: 0.0 - min_vel_y: 0.0 - max_vel_x: 0.3 - max_vel_y: 0.0 - max_vel_theta: 1.0 - min_speed_xy: 0.0 - max_speed_xy: 0.05 - min_speed_theta: 0.0 - # Add high threshold velocity for turtlebot 3 issue. - # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 - acc_lim_x: 2.5 - acc_lim_y: 0.0 - acc_lim_theta: 3.2 - decel_lim_x: -2.5 - decel_lim_y: 0.0 - decel_lim_theta: -3.2 - vx_samples: 20 - vy_samples: 5 - vtheta_samples: 20 - sim_time: 1.7 - linear_granularity: 0.05 - angular_granularity: 0.025 - transform_tolerance: 0.2 - xy_goal_tolerance: 0.157 - yaw_goal_tolerance: 0.15 - trans_stopped_velocity: 0.25 - short_circuit_trajectory_evaluation: True - stateful: True - critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] - BaseObstacle.scale: 0.02 - PathAlign.scale: 32.0 - PathAlign.forward_point_distance: 0.1 - GoalAlign.scale: 24.0 - GoalAlign.forward_point_distance: 0.1 - PathDist.scale: 32.0 - GoalDist.scale: 24.0 - RotateToGoal.scale: 32.0 - RotateToGoal.slowing_factor: 5.0 - RotateToGoal.lookahead_time: -1.0 - BackwardLocalPlanner: - plugin: "backward_local_planner::BackwardLocalPlanner" - transform_tolerance: 0.5 - k_rho: -1.4 # linear distance to the goal gain, default = -1.0 (related with carrot distance) - k_alpha: -0.1 #-0.1 - k_betta: -0.1 # -1.0 - carrot_distance: 0.3 #meters default 0.5 - carrot_angular_distance: 0.3 # no constraint, max 3.1416 - pure_spinning_straight_line_mode: true - max_linear_x_speed: 0.25 - ForwardLocalPlanner: - plugin: "cl_nav2z::forward_local_planner::ForwardLocalPlanner" - transform_tolerance: 0.5 - k_rho: 2.5 - k_alpha: -0.4 - k_betta: -1.0 - carrot_distance: 0.2 #meters - carrot_angular_distance: 0.5 # no constraint, max 3.1416 - max_linear_x_speed: 0.3 - max_angular_z_speed: 0.2 - PureSpinningLocalPlanner: - plugin: "pure_spinning_local_planner::PureSpinningLocalPlanner" - transform_tolerance: 0.5 - k_betta: 10.0 - max_angular_z_speed: 0.2 - use_shortest_angular_distance: true - - -controller_server_rclcpp_node: - ros__parameters: - use_sim_time: True - -local_costmap: - local_costmap: - ros__parameters: - update_frequency: 5.0 - publish_frequency: 2.0 - global_frame: odom - robot_base_frame: base_link - use_sim_time: True - rolling_window: true - width: 3 - height: 3 - resolution: 0.05 - robot_radius: 0.22 - plugins: ["voxel_layer", "inflation_layer"] - inflation_layer: - plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 3.0 - inflation_radius: 0.55 - voxel_layer: - plugin: "nav2_costmap_2d::VoxelLayer" - enabled: True - publish_voxel_map: True - origin_z: 0.0 - z_resolution: 0.05 - z_voxels: 16 - max_obstacle_height: 2.0 - mark_threshold: 0 - observation_sources: scan - scan: - topic: /scan - max_obstacle_height: 2.0 - clearing: True - marking: True - data_type: "LaserScan" - raytrace_max_range: 15.0 - raytrace_min_range: 0.0 - obstacle_max_range: 15.5 - obstacle_min_range: 0.0 - static_layer: - map_subscribe_transient_local: True - always_send_full_costmap: True - local_costmap_client: - ros__parameters: - use_sim_time: True - local_costmap_rclcpp_node: - ros__parameters: - use_sim_time: True - -global_costmap: - global_costmap: - ros__parameters: - update_frequency: 1.0 - publish_frequency: 1.0 - transform_tolerance: 5.0 - global_frame: map - robot_base_frame: base_link - use_sim_time: True - robot_radius: 0.22 - resolution: 0.05 - track_unknown_space: true - plugins: ["static_layer", "obstacle_layer", "inflation_layer"] - obstacle_layer: - plugin: "nav2_costmap_2d::ObstacleLayer" - enabled: True - observation_sources: scan - scan: - topic: /scan - max_obstacle_height: 2.0 - clearing: True - marking: True - data_type: "LaserScan" - raytrace_max_range: 15.0 - raytrace_min_range: 0.0 - obstacle_max_range: 15.5 - obstacle_min_range: 0.0 - static_layer: - plugin: "nav2_costmap_2d::StaticLayer" - map_subscribe_transient_local: True - inflation_layer: - plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 3.0 - inflation_radius: 0.55 - always_send_full_costmap: True - global_costmap_client: - ros__parameters: - use_sim_time: True - global_costmap_rclcpp_node: - ros__parameters: - use_sim_time: True - -map_server: - ros__parameters: - use_sim_time: True - yaml_filename: "turtlebot3_world.yaml" - -map_saver: - ros__parameters: - use_sim_time: True - save_map_timeout: 5000 - free_thresh_default: 0.25 - occupied_thresh_default: 0.65 - -planner_server: - ros__parameters: - expected_planner_frequency: 20.0 - use_sim_time: True - planner_plugins: ["GridBased", "ForwardGlobalPlanner", "BackwardGlobalPlanner", "UndoPathGlobalPlanner"] - GridBased: - plugin: "nav2_navfn_planner/NavfnPlanner" - tolerance: 10.0 #meters - use_astar: false - allow_unknown: true - ForwardGlobalPlanner: - plugin: "forward_global_planner::ForwardGlobalPlanner" - transform_tolerance: 0.5 - BackwardGlobalPlanner: - plugin: "backward_global_planner::BackwardGlobalPlanner" - transform_tolerance: 0.5 - UndoPathGlobalPlanner: - plugin: "undo_path_global_planner::UndoPathGlobalPlanner" - transform_tolerance: 0.5 - -planner_server_rclcpp_node: - ros__parameters: - use_sim_time: True - -recoveries_server: - ros__parameters: - costmap_topic: local_costmap/costmap_raw - footprint_topic: local_costmap/published_footprint - cycle_frequency: 10.0 - recovery_plugins: ["spin", "backup", "wait"] - #recovery_plugins: ["spin"] - spin: - plugin: "nav2_recoveries/Spin" - backup: - plugin: "nav2_recoveries/BackUp" - wait: - plugin: "nav2_recoveries/Wait" - global_frame: odom - robot_base_frame: base_link - transform_timeout: 0.1 - use_sim_time: true - simulate_ahead_time: 2.0 - max_rotational_vel: 1.0 - min_rotational_vel: 0.4 - rotational_acc_lim: 3.2 - -robot_state_publisher: - ros__parameters: - use_sim_time: True diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client_husky/navigation_tree.xml b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client_husky/navigation_tree.xml deleted file mode 100644 index e599e5d4f..000000000 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client_husky/navigation_tree.xml +++ /dev/null @@ -1,27 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client_husky/waypoints_plan.yaml b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client_husky/waypoints_plan.yaml deleted file mode 100644 index 64adbe1b0..000000000 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client_husky/waypoints_plan.yaml +++ /dev/null @@ -1,82 +0,0 @@ -waypoints: - # POINT 0 - - position: - x: 3.0 - y: -2.0 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.0 - w: 1.0 - # POINT 1 - - position: - x: -5.75 - y: 3.5 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.0 - w: 1.0 - # POINT 2 - - position: - x: -10.25 - y: -4.25 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.0 - w: 1.0 - # POINT 3 - - position: - x: 2.0 - y: -8.58 # north west corner of the room - #y: -12.35 # south west corner of the room - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.0 - w: 1.0 - # POINT 4 - - position: - x: -12.00 - y: 12.0 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.0 - w: 1.0 - # POINT 5 - - position: - x: -10.0 - y: 14.5 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.0 - w: 1.0 - # POINT 6 - - position: - x: -0.0 - y: 0.0 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.0 - w: 1.0 - #POINT 7 - - position: - x: 6.0 - y: 8.0 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.0 - w: 1.0 diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client_turtlebot/nav2_params.yaml b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client_turtlebot/nav2_params.yaml deleted file mode 100644 index 57d8d63ad..000000000 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client_turtlebot/nav2_params.yaml +++ /dev/null @@ -1,369 +0,0 @@ -amcl: - ros__parameters: - use_sim_time: True - alpha1: 0.2 - alpha2: 0.2 - alpha3: 0.2 - alpha4: 0.2 - alpha5: 0.2 - base_frame_id: "base_footprint" - beam_skip_distance: 0.5 - beam_skip_error_threshold: 0.9 - beam_skip_threshold: 0.3 - do_beamskip: false - global_frame_id: "map" - lambda_short: 0.1 - laser_likelihood_max_dist: 2.0 - laser_max_range: 100.0 - laser_min_range: -1.0 - laser_model_type: "likelihood_field" - max_beams: 60 - max_particles: 2000 - min_particles: 500 - odom_frame_id: "odom" - pf_err: 0.05 - pf_z: 0.99 - recovery_alpha_fast: 0.0 - recovery_alpha_slow: 0.0 - resample_interval: 1 - robot_model_type: "differential" - save_pose_rate: 0.5 - sigma_hit: 0.2 - tf_broadcast: true - transform_tolerance: 1.0 - update_min_a: 0.2 - update_min_d: 0.25 - z_hit: 0.5 - z_max: 0.05 - z_rand: 0.5 - z_short: 0.05 - scan_topic: scan - -amcl_map_client: - ros__parameters: - use_sim_time: True - -amcl_rclcpp_node: - ros__parameters: - use_sim_time: True - -bt_navigator: - ros__parameters: - use_sim_time: True - global_frame: map - robot_base_frame: base_link - odom_topic: /odom - enable_groot_monitoring: True - groot_zmq_publisher_port: 1666 - groot_zmq_server_port: 1667 - default_nav_to_pose_bt_xml: RUNTIMEFILL - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_back_up_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - -bt_navigator_rclcpp_node: - ros__parameters: - use_sim_time: True - -controller_server: - ros__parameters: - use_sim_time: True - controller_frequency: 20.0 - min_x_velocity_threshold: 0.001 - min_y_velocity_threshold: 0.5 - min_theta_velocity_threshold: 0.001 - progress_checker_plugin: "progress_checker" - goal_checker_plugins: ["goal_checker", "forward_goal_checker", "backward_goal_checker", "absolute_rotate_goal_checker", "undo_path_backwards_goal_checker"] - controller_plugins: ["FollowPath","BackwardLocalPlanner", "ForwardLocalPlanner", "PureSpinningLocalPlanner"] - - # Progress checker parameters - progress_checker: - plugin: "nav2_controller::SimpleProgressChecker" - required_movement_radius: 0.001 - movement_time_allowance: 3000.0 - - # Goal checker parameters - goal_checker: - plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 0.257 - yaw_goal_tolerance: 0.165 # almost free orientation - stateful: True - backward_goal_checker: - plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 0.09 - yaw_goal_tolerance: 0.05 - stateful: True - forward_goal_checker: - plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 0.09 - yaw_goal_tolerance: 0.05 - stateful: True - absolute_rotate_goal_checker: - plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 0.15 - yaw_goal_tolerance: 0.01 #0.005 - stateful: True - undo_path_backwards_goal_checker: - plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 0.03 # 4 cm - yaw_goal_tolerance: 0.1 - stateful: True - - # DWB parameters - FollowPath: - plugin: "dwb_core::DWBLocalPlanner" - debug_trajectory_details: True - min_vel_x: 0.0 - min_vel_y: 0.0 - max_vel_x: 0.3 - max_vel_y: 0.0 - max_vel_theta: 1.0 - min_speed_xy: 0.0 - max_speed_xy: 0.3 - min_speed_theta: 0.0 - # Add high threshold velocity for turtlebot 3 issue. - # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 - acc_lim_x: 2.5 - acc_lim_y: 0.0 - acc_lim_theta: 3.2 - decel_lim_x: -2.5 - decel_lim_y: 0.0 - decel_lim_theta: -3.2 - vx_samples: 20 - vy_samples: 5 - vtheta_samples: 20 - sim_time: 1.7 - linear_granularity: 0.05 - angular_granularity: 0.025 - transform_tolerance: 0.2 - xy_goal_tolerance: 0.157 - yaw_goal_tolerance: 0.15 - trans_stopped_velocity: 0.25 - short_circuit_trajectory_evaluation: True - stateful: True - critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] - BaseObstacle.scale: 0.02 - PathAlign.scale: 32.0 - PathAlign.forward_point_distance: 0.1 - GoalAlign.scale: 24.0 - GoalAlign.forward_point_distance: 0.1 - PathDist.scale: 32.0 - GoalDist.scale: 24.0 - RotateToGoal.scale: 32.0 - RotateToGoal.slowing_factor: 5.0 - RotateToGoal.lookahead_time: -1.0 - BackwardLocalPlanner: - plugin: "backward_local_planner::BackwardLocalPlanner" - transform_tolerance: 0.5 - k_rho: -1.4 # linear distance to the goal gain, default = -1.0 (related with carrot distance) - k_alpha: -0.1 #-0.1 - k_betta: -0.1 # -1.0 - carrot_distance: 0.3 #meters default 0.5 - carrot_angular_distance: 0.3 # no constraint, max 3.1416 - pure_spinning_straight_line_mode: true - max_linear_x_speed: 0.25 - ForwardLocalPlanner: - plugin: "cl_nav2z::forward_local_planner::ForwardLocalPlanner" - transform_tolerance: 0.5 - k_rho: 2.5 - k_alpha: -0.4 - k_betta: -1.0 - carrot_distance: 0.2 #meters - carrot_angular_distance: 0.5 # no constraint, max 3.1416 - max_linear_x_speed: 0.3 - max_angular_z_speed: 0.2 - PureSpinningLocalPlanner: - plugin: "pure_spinning_local_planner::PureSpinningLocalPlanner" - transform_tolerance: 0.5 - k_betta: 10.0 - max_angular_z_speed: 0.2 - use_shortest_angular_distance: true - - -controller_server_rclcpp_node: - ros__parameters: - use_sim_time: True - -local_costmap: - local_costmap: - ros__parameters: - update_frequency: 5.0 - publish_frequency: 2.0 - global_frame: odom - robot_base_frame: base_link - use_sim_time: True - rolling_window: true - width: 6 - height: 6 - resolution: 0.05 - robot_radius: 0.22 - plugins: ["voxel_layer", "inflation_layer"] - inflation_layer: - plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 3.0 - inflation_radius: 0.55 - voxel_layer: - plugin: "nav2_costmap_2d::VoxelLayer" - enabled: True - publish_voxel_map: True - origin_z: 0.0 - z_resolution: 0.05 - z_voxels: 16 - max_obstacle_height: 2.0 - mark_threshold: 0 - observation_sources: scan - scan: - topic: /scan - max_obstacle_height: 2.0 - clearing: True - marking: True - data_type: "LaserScan" - raytrace_max_range: 15.0 - raytrace_min_range: 0.0 - obstacle_max_range: 15.5 - obstacle_min_range: 0.0 - static_layer: - map_subscribe_transient_local: True - always_send_full_costmap: True - local_costmap_client: - ros__parameters: - use_sim_time: True - local_costmap_rclcpp_node: - ros__parameters: - use_sim_time: True - -global_costmap: - global_costmap: - ros__parameters: - width: 8 - height: 8 - update_frequency: 1.0 - publish_frequency: 1.0 - transform_tolerance: 5.0 - global_frame: map - robot_base_frame: base_link - use_sim_time: True - robot_radius: 0.22 - resolution: 0.05 - track_unknown_space: true - plugins: ["static_layer", "obstacle_layer", "inflation_layer"] - obstacle_layer: - plugin: "nav2_costmap_2d::ObstacleLayer" - enabled: True - observation_sources: scan - scan: - topic: /scan - max_obstacle_height: 2.0 - clearing: True - marking: True - data_type: "LaserScan" - raytrace_max_range: 15.0 - raytrace_min_range: 0.0 - obstacle_max_range: 15.5 - obstacle_min_range: 0.0 - static_layer: - plugin: "nav2_costmap_2d::StaticLayer" - map_subscribe_transient_local: True - inflation_layer: - plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 3.0 - inflation_radius: 0.55 - always_send_full_costmap: True - global_costmap_client: - ros__parameters: - use_sim_time: True - global_costmap_rclcpp_node: - ros__parameters: - use_sim_time: True - -map_server: - ros__parameters: - use_sim_time: True - yaml_filename: "turtlebot3_world.yaml" - -map_saver: - ros__parameters: - use_sim_time: True - save_map_timeout: 5000 - free_thresh_default: 0.25 - occupied_thresh_default: 0.65 - -planner_server: - ros__parameters: - expected_planner_frequency: 20.0 - use_sim_time: True - planner_plugins: ["GridBased", "ForwardGlobalPlanner", "BackwardGlobalPlanner", "UndoPathGlobalPlanner"] - GridBased: - plugin: "nav2_navfn_planner/NavfnPlanner" - tolerance: 10.0 #meters - use_astar: false - allow_unknown: true - ForwardGlobalPlanner: - plugin: "forward_global_planner::ForwardGlobalPlanner" - transform_tolerance: 0.5 - BackwardGlobalPlanner: - plugin: "backward_global_planner::BackwardGlobalPlanner" - transform_tolerance: 0.5 - UndoPathGlobalPlanner: - plugin: "undo_path_global_planner::UndoPathGlobalPlanner" - transform_tolerance: 0.5 - -planner_server_rclcpp_node: - ros__parameters: - use_sim_time: True - -recoveries_server: - ros__parameters: - costmap_topic: local_costmap/costmap_raw - footprint_topic: local_costmap/published_footprint - cycle_frequency: 10.0 - recovery_plugins: ["spin", "backup", "wait"] - #recovery_plugins: ["spin"] - spin: - plugin: "nav2_recoveries/Spin" - backup: - plugin: "nav2_recoveries/BackUp" - wait: - plugin: "nav2_recoveries/Wait" - global_frame: odom - robot_base_frame: base_link - transform_timeout: 0.1 - use_sim_time: true - simulate_ahead_time: 2.0 - max_rotational_vel: 1.0 - min_rotational_vel: 0.4 - rotational_acc_lim: 3.2 - -robot_state_publisher: - ros__parameters: - use_sim_time: True diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client_turtlebot/navigation_tree.xml b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client_turtlebot/navigation_tree.xml deleted file mode 100644 index e599e5d4f..000000000 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client_turtlebot/navigation_tree.xml +++ /dev/null @@ -1,27 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client_turtlebot/waypoints_plan.yaml b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client_turtlebot/waypoints_plan.yaml deleted file mode 100644 index 64adbe1b0..000000000 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client_turtlebot/waypoints_plan.yaml +++ /dev/null @@ -1,82 +0,0 @@ -waypoints: - # POINT 0 - - position: - x: 3.0 - y: -2.0 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.0 - w: 1.0 - # POINT 1 - - position: - x: -5.75 - y: 3.5 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.0 - w: 1.0 - # POINT 2 - - position: - x: -10.25 - y: -4.25 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.0 - w: 1.0 - # POINT 3 - - position: - x: 2.0 - y: -8.58 # north west corner of the room - #y: -12.35 # south west corner of the room - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.0 - w: 1.0 - # POINT 4 - - position: - x: -12.00 - y: 12.0 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.0 - w: 1.0 - # POINT 5 - - position: - x: -10.0 - y: 14.5 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.0 - w: 1.0 - # POINT 6 - - position: - x: -0.0 - y: 0.0 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.0 - w: 1.0 - #POINT 7 - - position: - x: 6.0 - y: 8.0 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.0 - w: 1.0 From 0f448aaff127d3ec741863f1d954ceb8f0242b86 Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Fri, 11 Aug 2023 12:04:56 +0200 Subject: [PATCH 53/70] more changes --- .../states/st_back_on_road_waypoints_x.hpp | 68 ------------------- .../states/st_inital_road_waypoints_x.hpp | 64 ----------------- .../states/st_inital_road_waypoints_x.hpp | 2 +- 3 files changed, 1 insertion(+), 133 deletions(-) delete mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgalery_ue/states/st_back_on_road_waypoints_x.hpp delete mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgalery_ue/states/st_inital_road_waypoints_x.hpp diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgalery_ue/states/st_back_on_road_waypoints_x.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgalery_ue/states/st_back_on_road_waypoints_x.hpp deleted file mode 100644 index 0130dfd1c..000000000 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgalery_ue/states/st_back_on_road_waypoints_x.hpp +++ /dev/null @@ -1,68 +0,0 @@ -// 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 - -namespace sm_dancebot_artgallery_ue -{ - -// STATE DECLARATION -struct StBackOnRoadWaypointsX : smacc2::SmaccState -{ - using SmaccState::SmaccState; - - // CUSTOM TRANSITION TAGS - struct TRANSITION_1 : SUCCESS{}; - struct TRANSITION_2 : SUCCESS{}; - struct TRANSITION_3 : SUCCESS{}; - struct TRANSITION_4 : SUCCESS{}; - struct TRANSITION_5 : SUCCESS{}; - struct TRANSITION_6 : SUCCESS{}; - - // TRANSITION TABLE - typedef mpl::list< - Transition, - Transition, StBackOnRoadWaypointsX, TRANSITION_1> - // Transition, StNavigateToWaypointsX, TRANSITION_2>, - // Transition, SS1::SsRadialPattern1, TRANSITION_3>, - // Transition, SS2::SsRadialPattern2, TRANSITION_4> - >reactions; - - // STATE FUNCTIONS - static void staticConfigure() - { - // configure_orthogonal(); - configure_orthogonal("waypoints_plan_back_on_road", "sm_dancebot_artgallery_ue"); - configure_orthogonal(); - } - - void onEntry() - { - } - - void runtimeConfigure() {} - - void onExit(ABORT) - { - } -}; -} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgalery_ue/states/st_inital_road_waypoints_x.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgalery_ue/states/st_inital_road_waypoints_x.hpp deleted file mode 100644 index f838c0637..000000000 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgalery_ue/states/st_inital_road_waypoints_x.hpp +++ /dev/null @@ -1,64 +0,0 @@ -// 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 - -namespace sm_dancebot_artgallery_ue -{ - -// STATE DECLARATION -struct StInitialRoadWaypointsX : smacc2::SmaccState -{ - using SmaccState::SmaccState; - - // CUSTOM TRANSITION TAGS - struct TRANSITION_1 : SUCCESS{}; - struct TRANSITION_2 : SUCCESS{}; - struct TRANSITION_3 : SUCCESS{}; - struct TRANSITION_4 : SUCCESS{}; - struct TRANSITION_5 : SUCCESS{}; - struct TRANSITION_6 : SUCCESS{}; - - // TRANSITION TABLE - typedef mpl::list< - Transition, - //Transition, StTurnAround, TRANSITION_1>, - Transition, StInitialRoadWaypointsX, TRANSITION_2> - >reactions; - - // STATE FUNCTIONS - static void staticConfigure() - { - configure_orthogonal(); - } - - void onEntry() - { - } - - void runtimeConfigure() {} - - void onExit(ABORT) - { - } -}; -} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_inital_road_waypoints_x.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_inital_road_waypoints_x.hpp index 186b80df6..da3f1206f 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_inital_road_waypoints_x.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_inital_road_waypoints_x.hpp @@ -42,7 +42,7 @@ struct StInitialRoadWaypointsX : smacc2::SmaccState, //Transition, StTurnAround, TRANSITION_1>, - Transition, StInitialRoadWaypointsX, TRANSITION_2> + Transition, StInitialRoadWaypointsX, TRANSITION_2> >reactions; // STATE FUNCTIONS From a5a123fb6719f3ff415eafe3485a390d2bd3f485 Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Fri, 11 Aug 2023 12:54:04 +0200 Subject: [PATCH 54/70] minor --- ..._navigation_frames_ground_truth_adapter.py | 67 +++++++++++++++++++ 1 file changed, 67 insertions(+) create mode 100755 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/scripts/ue_navigation_frames_ground_truth_adapter.py diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/scripts/ue_navigation_frames_ground_truth_adapter.py b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/scripts/ue_navigation_frames_ground_truth_adapter.py new file mode 100755 index 000000000..d966ec5bc --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/scripts/ue_navigation_frames_ground_truth_adapter.py @@ -0,0 +1,67 @@ +#!/usr/bin/env python3 + +# 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 rclpy +from rclpy.node import Node +import tf2_ros +from geometry_msgs.msg import TransformStamped +import rosgraph_msgs.msg +import ue_msgs.msg + + +class StaticTransformPublisher: + def __init__(self, parent_frame, child_frame): + self.node = rclpy.create_node("static_transform_publisher_1") + self.transform_broadcaster = tf2_ros.TransformBroadcaster(node) + self.clock_msg = None + print("Initializing static transform publisher") + + self.uemsgs_sub = node.create_subscription( + ue_msgs.msg.EntityState, + "/ue_ros/model_state", + self.uemsgs_callback, + + # reliability reliable + rclpy.qos.qos_profile_services_default + ) + + self.clock_sub = node.create_subscription( + rosgraph_msgs.msg.Clock, + "/clock", + self.clock_callback, + qos_profile=rclpy.qos.qos_profile_sensor_data, + ) + + def clock_callback(self, msg): + clock_msg = msg + + def clock_callback(self, msg): + self.clock_msg = msg + + +def main(args=None): + print("Initializing static transform publisher") + rclpy.init() + + rclpy.spin(node) + rclpy.shutdown() + + +if __name__ == "__main__": + import sys + print("Starting static transform publisher") + + main() From 1a31345043a9f2e7fe79a8571555885b1dc157b6 Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Fri, 11 Aug 2023 13:12:00 +0200 Subject: [PATCH 55/70] minor --- .../sm_dancebot_artgallery_ue/CMakeLists.txt | 1 + ..._navigation_frames_ground_truth_adapter.py | 35 +++++++++++++++---- 2 files changed, 29 insertions(+), 7 deletions(-) diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/CMakeLists.txt b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/CMakeLists.txt index 7c8da8a7b..0a41dc3ef 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/CMakeLists.txt +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/CMakeLists.txt @@ -100,6 +100,7 @@ install(TARGETS install(FILES servers/service_node_3/service_node_3.py scripts/transform_publisher.py + scripts/ue_navigation_frames_ground_truth_adapter.py DESTINATION lib/${PROJECT_NAME} PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READ diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/scripts/ue_navigation_frames_ground_truth_adapter.py b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/scripts/ue_navigation_frames_ground_truth_adapter.py index d966ec5bc..15c21c617 100755 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/scripts/ue_navigation_frames_ground_truth_adapter.py +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/scripts/ue_navigation_frames_ground_truth_adapter.py @@ -22,14 +22,14 @@ import ue_msgs.msg -class StaticTransformPublisher: +class UENavigationFramesGroundTruthAdapter: def __init__(self, parent_frame, child_frame): self.node = rclpy.create_node("static_transform_publisher_1") - self.transform_broadcaster = tf2_ros.TransformBroadcaster(node) + self.transform_broadcaster = tf2_ros.TransformBroadcaster(self.node) self.clock_msg = None print("Initializing static transform publisher") - self.uemsgs_sub = node.create_subscription( + self.uemsgs_sub = self.node.create_subscription( ue_msgs.msg.EntityState, "/ue_ros/model_state", self.uemsgs_callback, @@ -38,7 +38,7 @@ def __init__(self, parent_frame, child_frame): rclpy.qos.qos_profile_services_default ) - self.clock_sub = node.create_subscription( + self.clock_sub = self.node.create_subscription( rosgraph_msgs.msg.Clock, "/clock", self.clock_callback, @@ -48,15 +48,36 @@ def __init__(self, parent_frame, child_frame): def clock_callback(self, msg): clock_msg = msg - def clock_callback(self, msg): - self.clock_msg = msg + def uemsgs_callback(self, msg): + if self.clock_msg is None: + return + + t = TransformStamped() + t.header.stamp = self.clock_msg.clock + t.header.frame_id = self.parent_frame + + t.child_frame_id = self.child_frame + t.transform.translation.x = msg.pose.position.x + t.transform.translation.y = msg.pose.position.y + t.transform.translation.z = msg.pose.position.z + + t.transform.rotation.x = msg.pose.orientation.x + t.transform.rotation.y = msg.pose.orientation.y + t.transform.rotation.z = msg.pose.orientation.z + t.transform.rotation.w = msg.pose.orientation.w + + def spin(self): + rclpy.spin(self.node) def main(args=None): print("Initializing static transform publisher") rclpy.init() - rclpy.spin(node) + ue_navigation_frames_ground_truth_adapter = UENavigationFramesGroundTruthAdapter( + "odom", "base_footprint" + ) + ue_navigation_frames_ground_truth_adapter.spin() rclpy.shutdown() From 5e4fe0ba9afb69622b5428be69f6197956f95b3a Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Fri, 11 Aug 2023 13:18:36 +0200 Subject: [PATCH 56/70] minor --- .../scripts/ue_navigation_frames_ground_truth_adapter.py | 1 - 1 file changed, 1 deletion(-) diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/scripts/ue_navigation_frames_ground_truth_adapter.py b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/scripts/ue_navigation_frames_ground_truth_adapter.py index 15c21c617..06b059f96 100755 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/scripts/ue_navigation_frames_ground_truth_adapter.py +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/scripts/ue_navigation_frames_ground_truth_adapter.py @@ -84,5 +84,4 @@ def main(args=None): if __name__ == "__main__": import sys print("Starting static transform publisher") - main() From 797e68c7029ff2541b9ef272834a7d71cdebf46b Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Fri, 11 Aug 2023 14:52:06 +0200 Subject: [PATCH 57/70] navigation parameters --- .../sm_dancebot_artgallery_ue_launch.py | 29 ++++----------- .../params/nav2z_client/nav2_params.yaml | 12 +++---- .../rviz/nav2_default_view.rviz | 23 ++++++------ .../scripts/transform_publisher.py | 35 ++++++++++--------- ..._navigation_frames_ground_truth_adapter.py | 14 +++++++- 5 files changed, 55 insertions(+), 58 deletions(-) diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/sm_dancebot_artgallery_ue_launch.py b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/sm_dancebot_artgallery_ue_launch.py index 3717a7175..f84fc691b 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/sm_dancebot_artgallery_ue_launch.py +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/sm_dancebot_artgallery_ue_launch.py @@ -79,7 +79,7 @@ def generate_launch_description(): ) declare_slam_cmd = DeclareLaunchArgument( - "slam", default_value="True", description="Whether run a SLAM" + "slam", default_value="False", description="Whether run a SLAM" ) declare_use_sim_time_cmd = DeclareLaunchArgument( @@ -208,29 +208,15 @@ def generate_launch_description(): arguments=["--ros-args", "--log-level", "INFO"], ) - led_action_server_node = Node( - package="sm_dancebot_artgallery_ue", - executable="led_action_server_node", - output="screen", - prefix=xtermprefix, - ) - temperature_action_server = Node( + gt_transform_publisher = Node( package="sm_dancebot_artgallery_ue", - executable="temperature_sensor_node", + executable="ue_navigation_frames_ground_truth_adapter.py", output="screen", prefix=xtermprefix, + parameters=[{"use_sim_time": use_sim_time}], ) - service3_node = Node( - package="sm_dancebot_artgallery_ue", - executable="service_node_3.py", - output="screen", - prefix=xtermprefix, - parameters=[ - {"autostart": True, "node_names": ["ss", "dfa"]}, - ], - ) # Create the launch description and populate ld = LaunchDescription() @@ -253,12 +239,9 @@ def generate_launch_description(): ld.add_action(declare_use_rviz_cmd) ld.add_action(sm_dance_bot_node) - # ld.add_action(service3_node) - # ld.add_action(temperature_action_server) - # ld.add_action(led_action_server_node) ld.add_action(static_transform_publisher) - # ld.add_action(static_transform_publisher_2) - + ld.add_action(gt_transform_publisher) + # # Add the actions to launch all of the navigation nodes ld.add_action(start_robot_state_publisher_cmd) ld.add_action(rviz_cmd) diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/nav2_params.yaml b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/nav2_params.yaml index d6a5fad47..21007c18a 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/nav2_params.yaml +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/nav2_params.yaml @@ -147,9 +147,9 @@ controller_server: min_vel_y: 0.0 max_vel_x: 0.3 max_vel_y: 0.0 - max_vel_theta: 0.5 + max_vel_theta: 1.5 min_speed_xy: 0.0 - max_speed_xy: 0.05 + max_speed_xy: 1.0 min_speed_theta: 0.0 # Add high threshold velocity for turtlebot 3 issue. # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 @@ -269,8 +269,8 @@ global_costmap: update_frequency: 2.0 publish_frequency: 1.0 transform_tolerance: 15.0 - width: 16 - height: 16 + width: 60 + height: 60 global_frame: map robot_base_frame: base_footprint use_sim_time: True @@ -278,10 +278,8 @@ global_costmap: robot_radius: 0.22 resolution: 0.1 - width: 8 - height: 8 track_unknown_space: true - plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + plugins: ["obstacle_layer", "inflation_layer"] # "static_layer" obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/rviz/nav2_default_view.rviz b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/rviz/nav2_default_view.rviz index e8183d2c2..83258787e 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/rviz/nav2_default_view.rviz +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/rviz/nav2_default_view.rviz @@ -6,6 +6,7 @@ Panels: Expanded: - /Global Options1 - /TF1 + - /TF1/Status1 - /TF1/Frames1 - /TF1/Tree1 - /Global Planner1/Global Costmap1 @@ -13,7 +14,7 @@ Panels: - /LaserScan1 - /LaserScan1/Topic1 Splitter Ratio: 0.5833333134651184 - Tree Height: 505 + Tree Height: 865 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -108,9 +109,9 @@ Visualization Manager: Value: true Marker Scale: 1 Name: TF - Show Arrows: false + Show Arrows: true Show Axes: true - Show Names: false + Show Names: true Tree: map: odom: @@ -711,9 +712,9 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 1263.3914794921875 + Max Intensity: 1327.652099609375 Min Color: 0; 0; 0 - Min Intensity: 634.1690063476562 + Min Intensity: 712.4454345703125 Name: LaserScan Position Transformer: XYZ Selectable: true @@ -777,21 +778,21 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Scale: -72.154541015625 + Scale: -66.80846405029297 Target Frame: base_link Value: TopDownOrtho (rviz_default_plugins) - X: 0.8163416981697083 - Y: -0.20031072199344635 + X: 14.238972663879395 + Y: -16.86465835571289 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 656 + Height: 1016 Hide Left Dock: false Hide Right Dock: true Navigation 2: collapsed: false - QMainWindow State: 000000ff00000000fd00000004000000000000028600000236fc020000000bfb0000001200530065006c0065006300740069006f006e000000003d000000830000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000236000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003200000002da000001010000013900fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000002800fffffffb0000001e005200650061006c00730065006e0073006500430061006d0065007200610100000177000000fc0000000000000000000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000022e0000023600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000002860000039efc020000000bfb0000001200530065006c0065006300740069006f006e000000003d000000830000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003200000002da000001010000013900fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000002800fffffffb0000001e005200650061006c00730065006e0073006500430061006d0065007200610100000177000000fc0000000000000000000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004ae0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 RealsenseCamera: collapsed: false Selection: @@ -800,6 +801,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1210 + Width: 1850 X: 70 Y: 27 diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/scripts/transform_publisher.py b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/scripts/transform_publisher.py index 8ff606892..4ffb492be 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/scripts/transform_publisher.py +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/scripts/transform_publisher.py @@ -22,13 +22,15 @@ tfbrod = None clock_msg = None + + class StaticTransformPublisher: def __init__(self, node, parent_frame, child_frame, xyz=[0.0, 0.0, 0.0], rpy=[0.0, 0.0, 0.0]): self.node = node global tfbrod print("Initializing static transform publisher") - self.transform_broadcaster =tfbrod + self.transform_broadcaster = tfbrod print("xyz: ", xyz) try: @@ -49,23 +51,24 @@ def __init__(self, node, parent_frame, child_frame, xyz=[0.0, 0.0, 0.0], rpy=[0. self.timer = node.create_timer(0.1, self.publish_transform) - def publish_transform(self): global clock_msg if clock_msg is None: return - - time = rclpy.time.Time(seconds = clock_msg.clock.sec, nanoseconds = clock_msg.clock.nanosec) - #time = time + rclpy.time.Duration(seconds=0.01) + + time = rclpy.time.Time(seconds=clock_msg.clock.sec, + nanoseconds=clock_msg.clock.nanosec) + # time = time + rclpy.time.Duration(seconds=0.01) self.transform.header.stamp = time.to_msg() self.transform_broadcaster.sendTransform(self.transform) - self.node.get_logger().info("Publishing transform from %s to %s" % (self.transform.header.frame_id, self.transform.child_frame_id)) + self.node.get_logger().info("Publishing transform from %s to %s" % + (self.transform.header.frame_id, self.transform.child_frame_id)) def main(args=None): print("Initializing static transform publisher") rclpy.init() - + node = rclpy.create_node("static_transform_publisher_1") global tfbrod @@ -76,7 +79,6 @@ def callback(msg): global clock_msg clock_msg = msg - clock_sub = node.create_subscription( rosgraph_msgs.msg.Clock, "/clock", @@ -87,13 +89,13 @@ def callback(msg): print("Creating static transform publisher nodes") # Create the first instance of StaticTransformPublisher node1 = StaticTransformPublisher( - node, + node, "base_footprint", "base_link", ) # - # Create the second instance of StaticTransformPublisher + # Create the second instance of StaticTransformPublisher node2 = StaticTransformPublisher( node, "base_link", @@ -101,11 +103,12 @@ def callback(msg): xyz=[-0.064, 0.0, 0.122], ) - # node3 = StaticTransformPublisher( - # node, - # "map", - # "odom" - # ) + node3 = StaticTransformPublisher( + node, + "map", + "odom", + xyz=[0.0, 0.0, 0.0], + ) rclpy.spin(node) rclpy.shutdown() @@ -114,5 +117,5 @@ def callback(msg): if __name__ == "__main__": import sys print("Starting static transform publisher") - + main() diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/scripts/ue_navigation_frames_ground_truth_adapter.py b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/scripts/ue_navigation_frames_ground_truth_adapter.py index 06b059f96..e5065f284 100755 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/scripts/ue_navigation_frames_ground_truth_adapter.py +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/scripts/ue_navigation_frames_ground_truth_adapter.py @@ -27,6 +27,10 @@ def __init__(self, parent_frame, child_frame): self.node = rclpy.create_node("static_transform_publisher_1") self.transform_broadcaster = tf2_ros.TransformBroadcaster(self.node) self.clock_msg = None + + self.parent_frame = parent_frame + self.child_frame = child_frame + print("Initializing static transform publisher") self.uemsgs_sub = self.node.create_subscription( @@ -46,12 +50,16 @@ def __init__(self, parent_frame, child_frame): ) def clock_callback(self, msg): - clock_msg = msg + self.clock_msg = msg def uemsgs_callback(self, msg): + + self.node.get_logger().info("uemsgs_callback" + str(msg)) + if self.clock_msg is None: return + t = TransformStamped() t.header.stamp = self.clock_msg.clock t.header.frame_id = self.parent_frame @@ -66,6 +74,10 @@ def uemsgs_callback(self, msg): t.transform.rotation.z = msg.pose.orientation.z t.transform.rotation.w = msg.pose.orientation.w + self.node.get_logger().info("uemsgs_callback" + str(t)) + + self.transform_broadcaster.sendTransform(t) + def spin(self): rclpy.spin(self.node) From 7caab7586c64796012bc7132d34d35b2579e3734 Mon Sep 17 00:00:00 2001 From: Administrator Date: Fri, 11 Aug 2023 12:54:39 +0000 Subject: [PATCH 58/70] =?UTF-8?q?A=C3=B1adidos=20waypoints=20y=20cambiado?= =?UTF-8?q?=20el=20nombre=20del=20nodo=20en=20el=20json=20de=20depuraci?= =?UTF-8?q?=C3=B3n=20que=20pod=C3=ADa=20dar=20fallo?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- smacc2_dev_tools/.vscode/launch.json | 2 +- .../nav2z_client/waypoints_art_gallery.yaml | 112 ++++++++++++++---- 2 files changed, 87 insertions(+), 27 deletions(-) diff --git a/smacc2_dev_tools/.vscode/launch.json b/smacc2_dev_tools/.vscode/launch.json index 7c4a9fa7a..273872a9d 100644 --- a/smacc2_dev_tools/.vscode/launch.json +++ b/smacc2_dev_tools/.vscode/launch.json @@ -381,7 +381,7 @@ "args": [ "--ros-args", "-r", - "__node:=SmDanceBot", + "__node:=SmDanceBotUE", "--params-file", "${workspaceFolder}/../install/sm_dancebot_ue/share/sm_dancebot_ue/params/sm_dancebot_ue_config.yaml" ], diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/waypoints_art_gallery.yaml b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/waypoints_art_gallery.yaml index d2ede7bf2..7e364b1f9 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/waypoints_art_gallery.yaml +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/waypoints_art_gallery.yaml @@ -11,8 +11,8 @@ waypoints: w: 1.0 # POINT R2 - position: - x: -21.0 - y: 40.0 + x: 2.850 + y: 52.0 z: 0.0 orientation: x: 0.0 @@ -21,8 +21,8 @@ waypoints: w: 1.0 # POINT R3 - position: - x: -21.0 - y: 63.0 + x: 2.85 + y: 42.0 z: 0.0 orientation: x: 0.0 @@ -31,8 +31,8 @@ waypoints: w: 1.0 # POINT R4 - position: - x: -2.0 - y: 67.0 + x: 7.85 + y: 36.2 z: 0.0 orientation: x: 0.0 @@ -41,8 +41,8 @@ waypoints: w: 1.0 # POINT R5 - position: - x: -10.5 - y: 63.0 + x: 7.85 + y: 24.0 z: 0.0 orientation: x: 0.0 @@ -51,8 +51,8 @@ waypoints: w: 1.0 # POINT R7 - position: - x: 2.0 - y: 35.0 + x: 1.40 + y: 26.0 z: 0.0 orientation: x: 0.0 @@ -61,8 +61,8 @@ waypoints: w: 1.0 # POINT R8 - position: - x: 3.0 - y: 46.0 + x: -3.70 + y: 26.70 z: 0.0 orientation: x: 0.0 @@ -71,8 +71,8 @@ waypoints: w: 1.0 # POINT R10 - position: - x: -30.88127197 - y: 53.87810547 + x: -8.0 + y: 43.0 z: 0.0 orientation: x: 0.0 @@ -81,8 +81,8 @@ waypoints: w: 1.0 # POINT R11 - position: - x: -16.21995605 - y: 35.0 + x: -29.7 + y: 43.0 z: 0.0 orientation: x: 0.0 @@ -91,8 +91,8 @@ waypoints: w: 1.0 # POINT R12 - position: - x: -28.99974121 - y: 66.22507324 + x: -29.7 + y: 65.20 z: 0.0 orientation: x: 0.0 @@ -101,8 +101,8 @@ waypoints: w: 1.0 # POINT R13 - position: - x: -3.86228577 - y: 52.00396973 + x: -35.15 + y: 65.2 z: 0.0 orientation: x: 0.0 @@ -111,8 +111,8 @@ waypoints: w: 1.0 # POINT R14 - position: - x: -38.07478027 - y: 63.84986816 + x: -35.15 + y: 70.50 z: 0.0 orientation: x: 0.0 @@ -121,8 +121,8 @@ waypoints: w: 1.0 # POINT R15 - position: - x: -38.07478027 - y: 48.30913086 + x: -22.70 + y: 70.50 z: 0.0 orientation: x: 0.0 @@ -131,11 +131,71 @@ waypoints: w: 1.0 # POINT R16 - position: - x: -7.86228577 - y: 41.00396973 + x: -20.80 + y: 63.90 z: 0.0 orientation: x: 0.0 y: -0.0 z: 0.0 w: 1.0 + # POINT R17 + - position: + x: 1.95 + y: 63.90 + z: 0.0 + orientation: + x: 0.0 + y: -0.0 + z: 0.0 + w: 1.0 + # POINT R18 + - position: + x: 1.95.80 + y: 71.15 + z: 0.0 + orientation: + x: 0.0 + y: -0.0 + z: 0.0 + w: 1.0 + # POINT R19 + - position: + x: -4.25 + y: 71.15 + z: 0.0 + orientation: + x: 0.0 + y: -0.0 + z: 0.0 + w: 1.0 + # POINT R20 + - position: + x: -4.25 + y: 62.00 + z: 0.0 + orientation: + x: 0.0 + y: -0.0 + z: 0.0 + w: 1.0 + # POINT R21 + - position: + x: -13.80 + y: 62.00 + z: 0.0 + orientation: + x: 0.0 + y: -0.0 + z: 0.0 + w: 1.0 + # POINT R22 + - position: + x: -15.50 + y: 51.0 + z: 0.0 + orientation: + x: 0.0 + y: -0.0 + z: 0.0 + w: 1.0 \ No newline at end of file From cfc6b7eb92b5f9e79bcefea01758dcdaa5a05bb0 Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Mon, 14 Aug 2023 08:49:05 +0200 Subject: [PATCH 59/70] progress state machine --- .../smacc2/impl/smacc_state_machine_impl.hpp | 2 +- .../cp_waypoints_navigator.cpp | 3 +- .../docker/frames_2023-08-05_01.06.53.gv | 2 +- .../cb_load_waypoints_file.hpp | 12 +- .../client_behaviors/cb_pure_spinning.hpp | 2 +- .../orthogonals/or_navigation.hpp | 2 +- .../sm_dancebot_artgallery_ue.hpp | 11 +- .../states/st_acquire_sensors.hpp | 9 +- .../states/st_inital_road_waypoints_x.hpp | 64 ---------- ...=> st_navigate_artgallery_waypoints.x.hpp} | 12 +- .../states/st_navigate_field_waypoints_x.hpp | 63 --------- .../states/st_turn_around.hpp | 54 -------- .../superstates/ss_f_pattern_1.hpp | 4 +- .../superstates/ss_radial_pattern_1.hpp | 3 +- .../superstates/ss_radial_pattern_2.hpp | 2 +- .../superstates/ss_radial_pattern_3.hpp | 1 + .../superstates/ss_s_pattern_1.hpp | 1 + .../launch/navigation_launch.py | 10 +- .../sm_dancebot_artgallery_ue_launch.py | 8 +- .../params/nav2z_client/nav2_params.yaml | 20 +-- .../nav2z_client/waypoints_art_gallery.yaml | 120 +++++++++--------- .../rviz/nav2_default_view.rviz | 44 +++---- .../scripts/transform_publisher.py | 12 +- ..._navigation_frames_ground_truth_adapter.py | 28 +++- .../client_behaviors/cb_pure_spinning.hpp | 2 +- 25 files changed, 167 insertions(+), 324 deletions(-) delete mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_inital_road_waypoints_x.hpp rename smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/{st_back_on_road_waypoints_x.hpp => st_navigate_artgallery_waypoints.x.hpp} (75%) delete mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_navigate_field_waypoints_x.hpp delete mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_turn_around.hpp diff --git a/smacc2/include/smacc2/impl/smacc_state_machine_impl.hpp b/smacc2/include/smacc2/impl/smacc_state_machine_impl.hpp index 4710a1c6e..df0630ec7 100644 --- a/smacc2/include/smacc2/impl/smacc_state_machine_impl.hpp +++ b/smacc2/include/smacc2/impl/smacc_state_machine_impl.hpp @@ -216,7 +216,7 @@ template void ISmaccStateMachine::postEvent(EventLifeTime evlifetime) { auto evname = smacc2::introspection::demangleSymbol(); - RCLCPP_DEBUG_STREAM(getLogger(), "Event " << evname); + RCLCPP_INFO_STREAM(getLogger(), "Event " << evname); auto * ev = new EventType(); this->postEvent(ev, evlifetime); } diff --git a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.cpp b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.cpp index ef31e3323..28357e2a0 100644 --- a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.cpp +++ b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.cpp @@ -154,7 +154,8 @@ void CpWaypointNavigatorBase::loadWaypointsFromYamlParameter( { // if it is the first time and the waypoints navigator is not configured std::string planfilepath; - getNode()->declare_parameter(parameter_name, planfilepath); + planfilepath = getNode()->declare_parameter(parameter_name, planfilepath); + RCLCPP_INFO(getLogger(), "waypoints plan parameter: %s", planfilepath.c_str()); if (getNode()->get_parameter(parameter_name, planfilepath)) { std::string package_share_directory = diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/frames_2023-08-05_01.06.53.gv b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/frames_2023-08-05_01.06.53.gv index 52e51c916..e40f10c56 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/frames_2023-08-05_01.06.53.gv +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/frames_2023-08-05_01.06.53.gv @@ -15,4 +15,4 @@ edge [style=invis]; subgraph cluster_legend { style=bold; color=black; label ="view_frames Result"; "Recorded at time: 1691190413.1841352"[ shape=plaintext ] ; }->"map"; -} \ No newline at end of file +} diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/cl_nav2z/client_behaviors/cb_load_waypoints_file.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/cl_nav2z/client_behaviors/cb_load_waypoints_file.hpp index 069f35d5c..c8a44946e 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/cl_nav2z/client_behaviors/cb_load_waypoints_file.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/cl_nav2z/client_behaviors/cb_load_waypoints_file.hpp @@ -33,7 +33,7 @@ struct CbLoadWaypointsFile : public smacc2::SmaccClientBehavior CbLoadWaypointsFile(std::string parameter_name, std::string packagenamesapce) : parameterName_(parameter_name), - packagenamesapce_(packagenamesapce) + packageNamespace_(packagenamesapce) { } @@ -42,10 +42,16 @@ struct CbLoadWaypointsFile : public smacc2::SmaccClientBehavior requiresComponent(waypointsNavigator_); // this is a component from the nav2z_client library if (filepath_) + { this->waypointsNavigator_->loadWayPointsFromFile(filepath_.value()); + } else + { + RCLCPP_INFO( + getLogger(), "Loading waypoints from parameter %s", parameterName_.value().c_str()); this->waypointsNavigator_->loadWaypointsFromYamlParameter( - parameterName_.value(), packagenamesapce_.value()); + parameterName_.value(), packageNamespace_.value()); + } // change this to skip some points of the yaml file, default = 0 waypointsNavigator_->currentWaypoint_ = 0; @@ -56,7 +62,7 @@ struct CbLoadWaypointsFile : public smacc2::SmaccClientBehavior std::optional filepath_; std::optional parameterName_; - std::optional packagenamesapce_; + std::optional packageNamespace_; cl_nav2z::CpWaypointNavigatorBase * waypointsNavigator_; }; diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/cl_nav2z/client_behaviors/cb_pure_spinning.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/cl_nav2z/client_behaviors/cb_pure_spinning.hpp index 5d678298e..c0acfa7b3 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/cl_nav2z/client_behaviors/cb_pure_spinning.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/cl_nav2z/client_behaviors/cb_pure_spinning.hpp @@ -123,4 +123,4 @@ struct CbPureSpinning : public smacc2::SmaccAsyncClientBehavior { } }; -} \ No newline at end of file +} diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/orthogonals/or_navigation.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/orthogonals/or_navigation.hpp index 494864314..6b1883e13 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/orthogonals/or_navigation.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/orthogonals/or_navigation.hpp @@ -62,7 +62,7 @@ class OrNavigation : public smacc2::Orthogonal // create waypoints navigator component auto waypointsNavigator = nav2zClient->createComponent(); - nav2zClient->createComponent("/ue_ros/map_origin_entity_state"); + // nav2zClient->createComponent("/ue_ros/map_origin_entity_state"); /*auto waypointsNavigator = */ // nav2zClient->createComponent<::cl_nav2z::CpWaypointNavigatorBase>(); diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/sm_dancebot_artgallery_ue.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/sm_dancebot_artgallery_ue.hpp index a0095f3d0..2541d17a1 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/sm_dancebot_artgallery_ue.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/sm_dancebot_artgallery_ue.hpp @@ -59,10 +59,7 @@ namespace sm_dancebot_artgallery_ue { //STATE FORWARD DECLARATIONS class StAcquireSensors; -class StInitialRoadWaypointsX; -class StNavigateFieldWaypointsX; -class StBackOnRoadWaypointsX; -class StTurnAround; +class StNavigateArtGalleryWaypointsX; class StFinalState; //SUPERSTATE FORWARD DECLARATIONS @@ -140,10 +137,8 @@ struct SmDanceBot : public smacc2::SmaccStateMachineBase //STATES +#include + #include -#include -#include -#include -#include #include diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_acquire_sensors.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_acquire_sensors.hpp index 4e0c9a4c2..50365f9dd 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_acquire_sensors.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_acquire_sensors.hpp @@ -41,7 +41,7 @@ struct StAcquireSensors : smacc2::SmaccState, StInitialRoadWaypointsX, ON_SENSORS_AVAILABLE>, + Transition, StNavigateArtGalleryWaypointsX, ON_SENSORS_AVAILABLE>, Transition >reactions; @@ -49,17 +49,10 @@ struct StAcquireSensors : smacc2::SmaccState("Hello World!"); - // configure_orthogonal(); - // configure_orthogonal(Service3Command::SERVICE3_ON); - // configure_orthogonal(); - // configure_orthogonal(); configure_orthogonal("waypoints_art_gallery_1", "sm_dancebot_artgallery_ue"); configure_orthogonal(5s); - - // Create State Reactor auto srAllSensorsReady = static_createStateReactor< SrAllEventsGo, smacc2::state_reactors::EvAllGo, diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_inital_road_waypoints_x.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_inital_road_waypoints_x.hpp deleted file mode 100644 index da3f1206f..000000000 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_inital_road_waypoints_x.hpp +++ /dev/null @@ -1,64 +0,0 @@ -// 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 - -namespace sm_dancebot_artgallery_ue -{ - -// STATE DECLARATION -struct StInitialRoadWaypointsX : smacc2::SmaccState -{ - using SmaccState::SmaccState; - - // CUSTOM TRANSITION TAGS - struct TRANSITION_1 : SUCCESS{}; - struct TRANSITION_2 : SUCCESS{}; - struct TRANSITION_3 : SUCCESS{}; - struct TRANSITION_4 : SUCCESS{}; - struct TRANSITION_5 : SUCCESS{}; - struct TRANSITION_6 : SUCCESS{}; - - // TRANSITION TABLE - typedef mpl::list< - Transition, - //Transition, StTurnAround, TRANSITION_1>, - Transition, StInitialRoadWaypointsX, TRANSITION_2> - >reactions; - - // STATE FUNCTIONS - static void staticConfigure() - { - configure_orthogonal(); - } - - void onEntry() - { - } - - void runtimeConfigure() {} - - void onExit(ABORT) - { - } -}; -} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_back_on_road_waypoints_x.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_navigate_artgallery_waypoints.x.hpp similarity index 75% rename from smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_back_on_road_waypoints_x.hpp rename to smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_navigate_artgallery_waypoints.x.hpp index 0130dfd1c..e7d2a1f33 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_back_on_road_waypoints_x.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_navigate_artgallery_waypoints.x.hpp @@ -26,7 +26,7 @@ namespace sm_dancebot_artgallery_ue { // STATE DECLARATION -struct StBackOnRoadWaypointsX : smacc2::SmaccState +struct StNavigateArtGalleryWaypointsX : smacc2::SmaccState { using SmaccState::SmaccState; @@ -41,9 +41,12 @@ struct StBackOnRoadWaypointsX : smacc2::SmaccState, - Transition, StBackOnRoadWaypointsX, TRANSITION_1> - // Transition, StNavigateToWaypointsX, TRANSITION_2>, - // Transition, SS1::SsRadialPattern1, TRANSITION_3>, + // Transition, SS5::SsSPattern1, SUCCESS>, + Transition, SS4::SsFPattern1, SUCCESS>, + Transition, SS1::SsRadialPattern1, SUCCESS>, + Transition, StNavigateArtGalleryWaypointsX, SUCCESS>, + Transition, StNavigateArtGalleryWaypointsX, ABORT>, + Transition, StNavigateArtGalleryWaypointsX, ABORT> // Transition, SS2::SsRadialPattern2, TRANSITION_4> >reactions; @@ -51,7 +54,6 @@ struct StBackOnRoadWaypointsX : smacc2::SmaccState(); - configure_orthogonal("waypoints_plan_back_on_road", "sm_dancebot_artgallery_ue"); configure_orthogonal(); } diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_navigate_field_waypoints_x.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_navigate_field_waypoints_x.hpp deleted file mode 100644 index b0e002970..000000000 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_navigate_field_waypoints_x.hpp +++ /dev/null @@ -1,63 +0,0 @@ -// 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 - -namespace sm_dancebot_artgallery_ue -{ - -// STATE DECLARATION -struct StNavigateFieldWaypointsX : smacc2::SmaccState -{ - using SmaccState::SmaccState; - - // CUSTOM TRANSITION TAGS - struct TRANSITION_1 : SUCCESS{}; - struct TRANSITION_2 : SUCCESS{}; - struct TRANSITION_3 : SUCCESS{}; - struct TRANSITION_4 : SUCCESS{}; - struct TRANSITION_5 : SUCCESS{}; - struct TRANSITION_6 : SUCCESS{}; - - // TRANSITION TABLE - typedef mpl::list< - Transition, StNavigateFieldWaypointsX, TRANSITION_1> - >reactions; - - // STATE FUNCTIONS - static void staticConfigure() - { - // configure_orthogonal(); - configure_orthogonal(); - } - - void onEntry() - { - } - - void runtimeConfigure() {} - - void onExit(ABORT) - { - } -}; -} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_turn_around.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_turn_around.hpp deleted file mode 100644 index a0606975e..000000000 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_turn_around.hpp +++ /dev/null @@ -1,54 +0,0 @@ -// 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 - -namespace sm_dancebot_artgallery_ue -{ -using namespace smacc2::default_events; -using smacc2::client_behaviors::CbSleepFor; -using namespace std::chrono_literals; - -// STATE DECLARATION -struct StTurnAround : smacc2::SmaccState -{ - using SmaccState::SmaccState; - - // TRANSITION TABLE - typedef mpl::list< - Transition, StBackOnRoadWaypointsX, SUCCESS> - >reactions; - - // STATE FUNCTIONS - static void staticConfigure() - { - configure_orthogonal(M_PI, 0.8); - } - - void runtimeConfigure() - { - auto spinningBehavior = this->getOrthogonal()->getClientBehavior(); - spinningBehavior->yaw_goal_tolerance_rads_ = 0.2; - - } -}; -} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_f_pattern_1.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_f_pattern_1.hpp index a4c0316a8..72f9be867 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_f_pattern_1.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_f_pattern_1.hpp @@ -56,7 +56,7 @@ struct SsFPattern1 // TRANSITION TABLE typedef mpl::list< - // Transition>, StNavigateReverse4, ENDLOOP> //, + Transition>, StNavigateArtGalleryWaypointsX, ENDLOOP> //, >reactions; @@ -64,7 +64,7 @@ struct SsFPattern1 // superstate parameters static constexpr float ray_lenght_meters() { return 3.25; } static constexpr float pitch_lenght_meters() { return 0.75; } - static constexpr int total_iterations() { return 10; } + static constexpr int total_iterations() { return 8; } static constexpr TDirection direction() { return TDirection::RIGHT; } // superstate state variables diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_radial_pattern_1.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_radial_pattern_1.hpp index 3607aa68d..40826e546 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_radial_pattern_1.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_radial_pattern_1.hpp @@ -47,6 +47,7 @@ struct SsRadialPattern1 // TRANSITION TABLE typedef mpl::list< + Transition, StNavigateArtGalleryWaypointsX, ENDLOOP> //, // Transition, StRotateDegrees1, ENDLOOP> // Transition, StNavigateReverse1, ENDLOOP> @@ -55,7 +56,7 @@ struct SsRadialPattern1 static constexpr int total_iterations() { return 16; } static constexpr float ray_angle_increment_degree() { return 360.0 / total_iterations(); } - static constexpr float ray_length_meters() { return 1.0; } + static constexpr float ray_length_meters() { return 4.0; } int iteration_count = 0; diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_radial_pattern_2.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_radial_pattern_2.hpp index 972649212..77994d6a3 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_radial_pattern_2.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_radial_pattern_2.hpp @@ -47,7 +47,7 @@ struct SsRadialPattern2 // TRANSITION TABLE typedef mpl::list< - // Transition, StNavigateReverse1, ENDLOOP> + Transition, StNavigateArtGalleryWaypointsX, ENDLOOP> >reactions; diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_radial_pattern_3.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_radial_pattern_3.hpp index 5ef0b9888..c97f03b85 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_radial_pattern_3.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_radial_pattern_3.hpp @@ -46,6 +46,7 @@ struct SsRadialPattern3 // TRANSITION TABLE typedef mpl::list< + Transition, StNavigateArtGalleryWaypointsX, ENDLOOP> //, // Transition, StRotateDegrees4, ENDLOOP> diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_s_pattern_1.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_s_pattern_1.hpp index 84e588c7e..bbbe37af4 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_s_pattern_1.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_s_pattern_1.hpp @@ -59,6 +59,7 @@ struct SsSPattern1 : smacc2::SmaccState, StNavigateReverse3, ENDLOOP> + Transition, StNavigateArtGalleryWaypointsX, ENDLOOP> //, >reactions; diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/navigation_launch.py b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/navigation_launch.py index f14edd17a..ec67fd974 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/navigation_launch.py +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/navigation_launch.py @@ -112,7 +112,15 @@ def generate_launch_description(): prefix=xtermprefix, parameters=[configured_params], remappings=remappings, - arguments=["--ros-args", "--log-level", "INFO"], + arguments=[ + "--ros-args", + "--log-level", + "INFO", + "--log-level", + "custom_planners:=DEBUG", + "--log-level", + "controller_server:=DEBUG", + ], ), Node( package="nav2_planner", diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/sm_dancebot_artgallery_ue_launch.py b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/sm_dancebot_artgallery_ue_launch.py index f84fc691b..9b32bec05 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/sm_dancebot_artgallery_ue_launch.py +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/sm_dancebot_artgallery_ue_launch.py @@ -142,7 +142,7 @@ def generate_launch_description(): parameters=[{"use_sim_time": use_sim_time}], remappings=remappings, arguments=[urdf], - prefix = xtermprefix + prefix=xtermprefix, ) static_transform_publisher = Node( @@ -193,7 +193,7 @@ def generate_launch_description(): executable="sm_dancebot_artgallery_ue_node", name="SmDanceBotArtGalleryUE", output="screen", - prefix=xtermprefix, + prefix=xtermprefix + " gdb -ex run --args", parameters=[ os.path.join( get_package_share_directory("sm_dancebot_artgallery_ue"), @@ -208,7 +208,6 @@ def generate_launch_description(): arguments=["--ros-args", "--log-level", "INFO"], ) - gt_transform_publisher = Node( package="sm_dancebot_artgallery_ue", executable="ue_navigation_frames_ground_truth_adapter.py", @@ -217,7 +216,6 @@ def generate_launch_description(): parameters=[{"use_sim_time": use_sim_time}], ) - # Create the launch description and populate ld = LaunchDescription() @@ -241,7 +239,7 @@ def generate_launch_description(): ld.add_action(sm_dance_bot_node) ld.add_action(static_transform_publisher) ld.add_action(gt_transform_publisher) - + # # Add the actions to launch all of the navigation nodes ld.add_action(start_robot_state_publisher_cmd) ld.add_action(rviz_cmd) diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/nav2_params.yaml b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/nav2_params.yaml index 21007c18a..8f8691cfd 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/nav2_params.yaml +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/nav2_params.yaml @@ -116,17 +116,17 @@ controller_server: goal_checker: plugin: "nav2_controller::SimpleGoalChecker" xy_goal_tolerance: 0.257 - yaw_goal_tolerance: 0.165 # almost free orientation + yaw_goal_tolerance: 1.65 # almost free orientation stateful: True backward_goal_checker: plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 0.09 - yaw_goal_tolerance: 0.05 + xy_goal_tolerance: 1.3 #0.9 + yaw_goal_tolerance: 0.1 #0.05 stateful: True forward_goal_checker: plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 0.09 - yaw_goal_tolerance: 0.05 + xy_goal_tolerance: 1.3 #0.09 + yaw_goal_tolerance: 0.1 #0.05 stateful: True absolute_rotate_goal_checker: plugin: "nav2_controller::SimpleGoalChecker" @@ -135,8 +135,8 @@ controller_server: stateful: True undo_path_backwards_goal_checker: plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 0.03 # 4 cm - yaw_goal_tolerance: 0.1 + xy_goal_tolerance: 0.04 # 4 cm + yaw_goal_tolerance: 0.14 stateful: True # DWB parameters @@ -145,11 +145,11 @@ controller_server: debug_trajectory_details: True min_vel_x: 0.0 min_vel_y: 0.0 - max_vel_x: 0.3 + max_vel_x: 0.4 max_vel_y: 0.0 max_vel_theta: 1.5 min_speed_xy: 0.0 - max_speed_xy: 1.0 + max_speed_xy: 0.4 min_speed_theta: 0.0 # Add high threshold velocity for turtlebot 3 issue. # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 @@ -198,7 +198,7 @@ controller_server: k_rho: 2.5 k_alpha: -0.4 k_betta: -1.0 - carrot_distance: 0.2 #meters + carrot_distance: 0.5 #meters carrot_angular_distance: 0.5 # no constraint, max 3.1416 max_linear_x_speed: 0.3 max_angular_z_speed: 0.2 diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/waypoints_art_gallery.yaml b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/waypoints_art_gallery.yaml index 7e364b1f9..1d1555c04 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/waypoints_art_gallery.yaml +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/waypoints_art_gallery.yaml @@ -1,64 +1,64 @@ waypoints: - # POINT R1 - - position: - x: 18.10907715 - y: 52.59328613 - z: 0.0 - orientation: - x: 0.0 - y: -0.0 - z: 0.0 - w: 1.0 - # POINT R2 - - position: - x: 2.850 - y: 52.0 - z: 0.0 - orientation: - x: 0.0 - y: -0.0 - z: 0.0 - w: 1.0 - # POINT R3 - - position: - x: 2.85 - y: 42.0 - z: 0.0 - orientation: - x: 0.0 - y: -0.0 - z: 0.0 - w: 1.0 - # POINT R4 - - position: - x: 7.85 - y: 36.2 - z: 0.0 - orientation: - x: 0.0 - y: -0.0 - z: 0.0 - w: 1.0 - # POINT R5 - - position: - x: 7.85 - y: 24.0 - z: 0.0 - orientation: - x: 0.0 - y: -0.0 - z: 0.0 - w: 1.0 + # # POINT R1 + # - position: + # x: 18.10907715 + # y: 52.59328613 + # z: 0.0 + # orientation: + # x: 0.0 + # y: -0.0 + # z: 0.0 + # w: 1.0 + # # POINT R2 + # - position: + # x: 2.850 + # y: 52.0 + # z: 0.0 + # orientation: + # x: 0.0 + # y: -0.0 + # z: 0.0 + # w: 1.0 + # # POINT R3 + # - position: + # x: 2.85 + # y: 42.0 + # z: 0.0 + # orientation: + # x: 0.0 + # y: -0.0 + # z: 0.0 + # w: 1.0 + # # POINT R4 + # - position: + # x: 7.85 + # y: 36.2 + # z: 0.0 + # orientation: + # x: 0.0 + # y: -0.0 + # z: 0.0 + # w: 1.0 + # # POINT R5 + # - position: + # x: 7.85 + # y: 24.0 + # z: 0.0 + # orientation: + # x: 0.0 + # y: -0.0 + # z: 0.0 + # w: 1.0 # POINT R7 - - position: - x: 1.40 - y: 26.0 - z: 0.0 - orientation: - x: 0.0 - y: -0.0 - z: 0.0 - w: 1.0 + # - position: + # x: 1.40 + # y: 26.0 + # z: 0.0 + # orientation: + # x: 0.0 + # y: -0.0 + # z: 0.0 + # w: 1.0 # POINT R8 - position: x: -3.70 @@ -198,4 +198,4 @@ waypoints: x: 0.0 y: -0.0 z: 0.0 - w: 1.0 \ No newline at end of file + w: 1.0 diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/rviz/nav2_default_view.rviz b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/rviz/nav2_default_view.rviz index 83258787e..6c26718d0 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/rviz/nav2_default_view.rviz +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/rviz/nav2_default_view.rviz @@ -6,12 +6,12 @@ Panels: Expanded: - /Global Options1 - /TF1 - - /TF1/Status1 - /TF1/Frames1 - /TF1/Tree1 + - /Global Planner1 - /Global Planner1/Global Costmap1 - - /Controller1/Local Costmap1 - - /LaserScan1 + - /Controller1 + - /Odometry1 - /LaserScan1/Topic1 Splitter Ratio: 0.5833333134651184 Tree Height: 865 @@ -109,9 +109,9 @@ Visualization Manager: Value: true Marker Scale: 1 Name: TF - Show Arrows: true + Show Arrows: false Show Axes: true - Show Names: true + Show Names: false Tree: map: odom: @@ -212,7 +212,7 @@ Visualization Manager: Value: true - Class: rviz_common/Group Displays: - - Alpha: 0.10000000149011612 + - Alpha: 0.4000000059604645 Class: rviz_default_plugins/Map Color Scheme: costmap Draw Behind: false @@ -312,7 +312,7 @@ Visualization Manager: Name: Global Planner - Class: rviz_common/Group Displays: - - Alpha: 0.10000000149011612 + - Alpha: 0.4000000059604645 Class: rviz_default_plugins/Map Color Scheme: costmap Draw Behind: false @@ -331,7 +331,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /local_costmap/costmap_updates - Use Timestamp: false + Use Timestamp: true Value: true - Alpha: 1 Buffer Length: 1 @@ -621,7 +621,7 @@ Visualization Manager: Enabled: true Name: ForwardCarrot Namespaces: - {} + my_namespace2: true Topic: Depth: 5 Durability Policy: Volatile @@ -706,20 +706,20 @@ Visualization Manager: Axis: Z Channel Name: intensity Class: rviz_default_plugins/LaserScan - Color: 255; 255; 255 - Color Transformer: Intensity + Color: 224; 27; 36 + Color Transformer: FlatColor Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 1327.652099609375 + Max Intensity: 1246.8863525390625 Min Color: 0; 0; 0 - Min Intensity: 712.4454345703125 + Min Intensity: 726.621337890625 Name: LaserScan Position Transformer: XYZ Selectable: true Size (Pixels): 3 - Size (m): 0.10000000149011612 + Size (m): 0.20000000298023224 Style: Flat Squares Topic: Depth: 5 @@ -768,7 +768,7 @@ Visualization Manager: Value: true Views: Current: - Angle: -4.659997940063477 + Angle: -1.7249938249588013 Class: rviz_default_plugins/TopDownOrtho Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 @@ -778,21 +778,21 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Scale: -66.80846405029297 + Scale: -175.2274627685547 Target Frame: base_link Value: TopDownOrtho (rviz_default_plugins) - X: 14.238972663879395 - Y: -16.86465835571289 + X: -0.12203919142484665 + Y: 0.4557923972606659 Saved: ~ Window Geometry: Displays: - collapsed: false + collapsed: true Height: 1016 - Hide Left Dock: false + Hide Left Dock: true Hide Right Dock: true Navigation 2: collapsed: false - QMainWindow State: 000000ff00000000fd0000000400000000000002860000039efc020000000bfb0000001200530065006c0065006300740069006f006e000000003d000000830000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003200000002da000001010000013900fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000002800fffffffb0000001e005200650061006c00730065006e0073006500430061006d0065007200610100000177000000fc0000000000000000000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004ae0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000002860000039efc020000000bfb0000001200530065006c0065006300740069006f006e000000003d000000830000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003200000002da000001010000013900fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000002800fffffffb0000001e005200650061006c00730065006e0073006500430061006d0065007200610100000177000000fc0000000000000000000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000039d0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 RealsenseCamera: collapsed: false Selection: @@ -801,6 +801,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1850 + Width: 925 X: 70 Y: 27 diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/scripts/transform_publisher.py b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/scripts/transform_publisher.py index 4ffb492be..2f9af0eef 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/scripts/transform_publisher.py +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/scripts/transform_publisher.py @@ -49,20 +49,21 @@ def __init__(self, node, parent_frame, child_frame, xyz=[0.0, 0.0, 0.0], rpy=[0. print(e) self.node.get_logger().error("Exception in StaticTransformPublisher: %r" % e) - self.timer = node.create_timer(0.1, self.publish_transform) + self.timer = node.create_timer(0.05, self.publish_transform) def publish_transform(self): global clock_msg if clock_msg is None: return - time = rclpy.time.Time(seconds=clock_msg.clock.sec, - nanoseconds=clock_msg.clock.nanosec) + time = rclpy.time.Time(seconds=clock_msg.clock.sec, nanoseconds=clock_msg.clock.nanosec) # time = time + rclpy.time.Duration(seconds=0.01) self.transform.header.stamp = time.to_msg() self.transform_broadcaster.sendTransform(self.transform) - self.node.get_logger().info("Publishing transform from %s to %s" % - (self.transform.header.frame_id, self.transform.child_frame_id)) + self.node.get_logger().info( + "Publishing transform from %s to %s" + % (self.transform.header.frame_id, self.transform.child_frame_id) + ) def main(args=None): @@ -116,6 +117,7 @@ def callback(msg): if __name__ == "__main__": import sys + print("Starting static transform publisher") main() diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/scripts/ue_navigation_frames_ground_truth_adapter.py b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/scripts/ue_navigation_frames_ground_truth_adapter.py index e5065f284..c2e5fe041 100755 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/scripts/ue_navigation_frames_ground_truth_adapter.py +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/scripts/ue_navigation_frames_ground_truth_adapter.py @@ -20,6 +20,7 @@ from geometry_msgs.msg import TransformStamped import rosgraph_msgs.msg import ue_msgs.msg +import nav_msgs.msg class UENavigationFramesGroundTruthAdapter: @@ -37,9 +38,12 @@ def __init__(self, parent_frame, child_frame): ue_msgs.msg.EntityState, "/ue_ros/model_state", self.uemsgs_callback, - # reliability reliable - rclpy.qos.qos_profile_services_default + rclpy.qos.qos_profile_services_default, + ) + + self.odom_pub = self.node.create_publisher( + nav_msgs.msg.Odometry, "/odom", rclpy.qos.qos_profile_services_default ) self.clock_sub = self.node.create_subscription( @@ -53,13 +57,10 @@ def clock_callback(self, msg): self.clock_msg = msg def uemsgs_callback(self, msg): - - self.node.get_logger().info("uemsgs_callback" + str(msg)) if self.clock_msg is None: return - t = TransformStamped() t.header.stamp = self.clock_msg.clock t.header.frame_id = self.parent_frame @@ -74,10 +75,24 @@ def uemsgs_callback(self, msg): t.transform.rotation.z = msg.pose.orientation.z t.transform.rotation.w = msg.pose.orientation.w - self.node.get_logger().info("uemsgs_callback" + str(t)) + # self.node.get_logger().info("uemsgs_callback" + str(t)) self.transform_broadcaster.sendTransform(t) + odom = nav_msgs.msg.Odometry() + odom.header.stamp = self.clock_msg.clock + odom.header.frame_id = self.parent_frame + odom.child_frame_id = self.child_frame + odom.pose.pose = msg.pose + + # set covariance diagonal to 0.1 + odom.pose.covariance = [0.1 if i % 7 == 0 else 0.0 for i in range(36)] + + odom.twist.twist = msg.twist + odom.twist.covariance = [0.1 if i % 7 == 0 else 0.0 for i in range(36)] + + self.odom_pub.publish(odom) + def spin(self): rclpy.spin(self.node) @@ -95,5 +110,6 @@ def main(args=None): if __name__ == "__main__": import sys + print("Starting static transform publisher") main() diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_pure_spinning.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_pure_spinning.hpp index 925c9334b..e11dd6aea 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_pure_spinning.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_pure_spinning.hpp @@ -123,4 +123,4 @@ struct CbPureSpinning : public smacc2::SmaccAsyncClientBehavior { } }; -} \ No newline at end of file +} From a94e9e75771713de2bcb8a5dc01f364abfa353a0 Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Mon, 14 Aug 2023 14:24:04 +0200 Subject: [PATCH 60/70] ue_projec_2 refactoring --- .../cp_waypoints_navigator.cpp | 2 +- .../sm_dancebot_artgallery_ue/CMakeLists.txt | 2 +- .../docker/attach_docker_container.sh | 2 +- .../docker/join_bash.sh | 2 +- .../docker/join_editor.sh | 2 +- .../docker/remove_container.sh | 2 +- .../docker/run_docker_container_bash.sh | 2 +- .../run_docker_container_bash_development.sh | 2 +- .../docker/run_docker_container_editor.sh | 2 +- .../docker/start_container.sh | 2 +- .../docker/stop_container.sh | 2 +- .../nav2z_client/cb_active_stop.cpp | 53 ------------------- .../nav2z_client/cb_active_stop.cpp | 3 +- .../sm_dancebot_ue/CMakeLists.txt | 2 +- .../nav2z_client/cb_active_stop.cpp | 2 +- 15 files changed, 15 insertions(+), 67 deletions(-) delete mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/src/sm_dancebot_artgalery_ue/clients/client_behaviors/nav2z_client/cb_active_stop.cpp diff --git a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.cpp b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.cpp index 28357e2a0..d263b763f 100644 --- a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.cpp +++ b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.cpp @@ -162,7 +162,7 @@ void CpWaypointNavigatorBase::loadWaypointsFromYamlParameter( ament_index_cpp::get_package_share_directory(yaml_file_package_name); RCLCPP_INFO(getLogger(), "file macro path: %s", planfilepath.c_str()); - + boost::replace_all(planfilepath, "$(pkg_share)", package_share_directory); RCLCPP_INFO(getLogger(), "package share path: %s", package_share_directory.c_str()); diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/CMakeLists.txt b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/CMakeLists.txt index 0a41dc3ef..053c0f50e 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/CMakeLists.txt +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/CMakeLists.txt @@ -59,7 +59,7 @@ add_executable(${PROJECT_NAME}_node src/${PROJECT_NAME}/clients/client_behaviors/nav2z_client/cb_position_control_free_space.cpp src/${PROJECT_NAME}/clients/components/cp_ue_pose.cpp src/${PROJECT_NAME}/clients/client_behaviors/nav2z_client/cb_active_stop.cpp - ) +) add_executable(temperature_sensor_node_${PROJECT_NAME} servers/temperature_sensor_node/src/temperature_sensor_node.cpp) add_executable(led_action_server_node_${PROJECT_NAME} servers/led_action_server/src/led_action_server_node.cpp) diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/attach_docker_container.sh b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/attach_docker_container.sh index 37343f136..7842a41d4 100755 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/attach_docker_container.sh +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/attach_docker_container.sh @@ -1 +1 @@ -sudo docker attach smacc_ue +sudo docker attach ue_editor_rclue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/join_bash.sh b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/join_bash.sh index bc3ad61e3..29ee10381 100755 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/join_bash.sh +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/join_bash.sh @@ -1 +1 @@ -sudo docker exec -it smacc_ue /bin/bash +sudo docker exec -it ue_editor_rclue /bin/bash diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/join_editor.sh b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/join_editor.sh index d215b6054..c11f2028e 100755 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/join_editor.sh +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/join_editor.sh @@ -1 +1 @@ -sudo docker exec -it smacc_ue ./run_editor_smacc.sh +sudo docker exec -it ue_editor_rclue ./run_editor_smacc.sh diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/remove_container.sh b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/remove_container.sh index 1247aae92..f83e1e49b 100755 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/remove_container.sh +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/remove_container.sh @@ -1 +1 @@ -sudo docker rm smacc_ue +sudo docker rm ue_editor_rclue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/run_docker_container_bash.sh b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/run_docker_container_bash.sh index 5ab294c08..a450ca485 100755 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/run_docker_container_bash.sh +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/run_docker_container_bash.sh @@ -5,4 +5,4 @@ DIR="$(dirname "$(realpath "$0")")" ROOT_DIR=`realpath $DIR/../../../..` DRIVER=$(apt list --installed | grep nvidia-driver) -sudo nvidia-docker run --net host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name smacc_ue -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash -c "./nvidia-driver-check.sh '$DRIVER' && bash" +sudo nvidia-docker run --net host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name ue_editor_rclue -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash -c "./nvidia-driver-check.sh '$DRIVER' && bash" diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/run_docker_container_bash_development.sh b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/run_docker_container_bash_development.sh index c446c50a5..86493df9b 100755 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/run_docker_container_bash_development.sh +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/run_docker_container_bash_development.sh @@ -5,4 +5,4 @@ DIR="$(dirname "$(realpath "$0")")" ROOT_DIR=`realpath $DIR/../../../..` DRIVER=$(apt list --installed | grep nvidia-driver) -sudo nvidia-docker run --net host --ipc=host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name smacc_ue -v $ROOT_DIR/ue_project_1:/home/ros2_ws/src/ue_project_1 -v $ROOT_DIR/SMACC2:/home/ros2_ws/src/SMACC2 -v $ROOT_DIR/UE-Plugins:/home/ros2_ws/src/ue_project_1/Plugins/RapyutaSimulationPlugins -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash -c "./nvidia-driver-check.sh '$DRIVER' && bash" +sudo nvidia-docker run --net host --ipc=host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name smacc_ue -v $ROOT_DIR/ue_project_1:/home/ros2_ws/src/ue_project_1 -v $ROOT_DIR/ue_project_2:/home/ros2_ws/src/ue_project_2 -v $ROOT_DIR/SMACC2:/home/ros2_ws/src/SMACC2 -v $ROOT_DIR/UE-Plugins:/home/ros2_ws/src/ue_project_1/Plugins/RapyutaSimulationPlugins -v $ROOT_DIR/rclUE:/home/ros2_ws/src/ue_project_1/Plugins/rclUE -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash -c "./nvidia-driver-check.sh '$DRIVER' && bash" diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/run_docker_container_editor.sh b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/run_docker_container_editor.sh index 2e93b8c7e..8dfad38c4 100755 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/run_docker_container_editor.sh +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/run_docker_container_editor.sh @@ -5,4 +5,4 @@ DIR="$(dirname "$(realpath "$0")")" ROOT_DIR=`realpath $DIR/../../../..` DRIVER=$(apt list --installed | grep nvidia-driver) -sudo nvidia-docker run --net host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name smacc_ue -v $ROOT_DIR/ue_project_2:/home/ros2_ws/src/ue_project_2 -v $ROOT_DIR/ue_project_1:/home/ros2_ws/src/ue_project_1 -v $ROOT_DIR/SMACC2:/home/ros2_ws/src/SMACC2 -v $ROOT_DIR/UE-Plugins:/home/ros2_ws/src/turtlebot3-UE/Plugins/RapyutaSimulationPlugins -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash -c "./nvidia-driver-check.sh '$DRIVER' && ./run_editor_smacc.sh" +sudo nvidia-docker run --net host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name ue_editor_rclue -v $ROOT_DIR/ue_project_2:/home/ros2_ws/src/ue_project_2 -v $ROOT_DIR/ue_project_1:/home/ros2_ws/src/ue_project_1 -v $ROOT_DIR/SMACC2:/home/ros2_ws/src/SMACC2 -v $ROOT_DIR/UE-Plugins:/home/ros2_ws/src/turtlebot3-UE/Plugins/RapyutaSimulationPlugins -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash -c "./nvidia-driver-check.sh '$DRIVER' && ./run_editor_smacc.sh" diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/start_container.sh b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/start_container.sh index ec24e9d12..6b339f906 100755 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/start_container.sh +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/start_container.sh @@ -1 +1 @@ -sudo docker start smacc_ue +sudo docker start ue_editor_rclue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/stop_container.sh b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/stop_container.sh index 3290cdb0c..0c06686cc 100755 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/stop_container.sh +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/stop_container.sh @@ -1 +1 @@ -sudo docker stop smacc_ue +sudo docker stop ue_editor_rclue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/src/sm_dancebot_artgalery_ue/clients/client_behaviors/nav2z_client/cb_active_stop.cpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/src/sm_dancebot_artgalery_ue/clients/client_behaviors/nav2z_client/cb_active_stop.cpp deleted file mode 100644 index cc14fe162..000000000 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/src/sm_dancebot_artgalery_ue/clients/client_behaviors/nav2z_client/cb_active_stop.cpp +++ /dev/null @@ -1,53 +0,0 @@ - -// 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 - * - ******************************************************************************************************************/ - -#include -#include -#include - -#include -#include - -namespace sm_dancebot_artgallery_ue -{ -CbActiveStop::CbActiveStop() {} - -void CbActiveStop::onEntry() -{ - auto nh = this->getNode(); - cmd_vel_pub_ = nh->create_publisher("/cmd_vel", rclcpp::QoS(1)); - - rclcpp::Rate loop_rate(5); - geometry_msgs::msg::Twist cmd_vel_msg; - while (!this->isShutdownRequested()) - { - cmd_vel_msg.linear.x = 0; - cmd_vel_msg.angular.z = 0; - - cmd_vel_pub_->publish(cmd_vel_msg); - loop_rate.sleep(); - } - RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] Finished behavior execution"); - - this->postSuccessEvent(); -} - -void CbActiveStop::onExit() {} - -} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/src/sm_dancebot_artgallery_ue/clients/client_behaviors/nav2z_client/cb_active_stop.cpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/src/sm_dancebot_artgallery_ue/clients/client_behaviors/nav2z_client/cb_active_stop.cpp index cc14fe162..8e8201148 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/src/sm_dancebot_artgallery_ue/clients/client_behaviors/nav2z_client/cb_active_stop.cpp +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/src/sm_dancebot_artgallery_ue/clients/client_behaviors/nav2z_client/cb_active_stop.cpp @@ -1,4 +1,5 @@ - +// 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 diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/CMakeLists.txt b/smacc2_sm_reference_library/sm_dancebot_ue/CMakeLists.txt index 1d16bacb2..4df9054cb 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/CMakeLists.txt +++ b/smacc2_sm_reference_library/sm_dancebot_ue/CMakeLists.txt @@ -59,7 +59,7 @@ add_executable(${PROJECT_NAME}_node src/${PROJECT_NAME}/clients/client_behaviors/nav2z_client/cb_position_control_free_space.cpp src/${PROJECT_NAME}/clients/components/cp_ue_pose.cpp src/${PROJECT_NAME}/clients/client_behaviors/nav2z_client/cb_active_stop.cpp - ) +) add_executable(temperature_sensor_node_${PROJECT_NAME} servers/temperature_sensor_node/src/temperature_sensor_node.cpp) add_executable(led_action_server_node_${PROJECT_NAME} servers/led_action_server/src/led_action_server_node.cpp) diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/clients/client_behaviors/nav2z_client/cb_active_stop.cpp b/smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/clients/client_behaviors/nav2z_client/cb_active_stop.cpp index a912ea9d0..727fa74d0 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/clients/client_behaviors/nav2z_client/cb_active_stop.cpp +++ b/smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/clients/client_behaviors/nav2z_client/cb_active_stop.cpp @@ -1,4 +1,4 @@ - +// 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 From e73301474bdb6197b9847c998b861b9b90e25326 Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Tue, 15 Aug 2023 00:36:25 +0200 Subject: [PATCH 61/70] tunning navigation --- .../client_behaviors/cb_absolute_rotate.cpp | 1 + .../client_behaviors/cb_navigate_forward.cpp | 7 +++-- .../docker/openvpn_file_run.sh | 2 +- .../run_docker_container_bash_development.sh | 2 +- .../sm_dancebot_artgallery_ue.hpp | 2 +- .../sti_fpattern_forward_1.hpp | 15 ++++++++-- .../sti_fpattern_rotate_1.hpp | 9 ++++-- .../sti_fpattern_rotate_2.hpp | 2 +- .../st_navigate_artgallery_waypoints.x.hpp | 2 +- .../superstates/ss_f_pattern_1.hpp | 4 ++- .../params/nav2z_client/nav2_params.yaml | 29 ++++++++++--------- .../nav2z_client/waypoints_art_gallery.yaml | 18 ++++++------ .../rviz/nav2_default_view.rviz | 26 +++++++++-------- .../docker/attach_docker_container.sh | 2 +- .../sm_dancebot_ue/docker/join_bash.sh | 2 +- .../sm_dancebot_ue/docker/join_editor.sh | 2 +- .../sm_dancebot_ue/docker/openvpn_file_run.sh | 2 +- .../sm_dancebot_ue/docker/remove_container.sh | 4 +-- .../docker/run_docker_container_bash.sh | 2 +- .../run_docker_container_bash_development.sh | 2 +- .../docker/run_docker_container_editor.sh | 2 +- .../sm_dancebot_ue/docker/start_container.sh | 2 +- .../sm_dancebot_ue/docker/stop_container.sh | 2 +- 23 files changed, 80 insertions(+), 61 deletions(-) diff --git a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_absolute_rotate.cpp b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_absolute_rotate.cpp index 4042eb827..83ce75dcf 100644 --- a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_absolute_rotate.cpp +++ b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_absolute_rotate.cpp @@ -82,6 +82,7 @@ void CbAbsoluteRotate::onEntry() auto pathname = this->getCurrentState()->getName() + " - " + getName(); odomTracker_->pushPath(pathname); odomTracker_->setStartPoint(p->toPoseStampedMsg()); + odomTracker_->setCurrentMotionGoal(goal.pose); odomTracker_->setWorkingMode(odom_tracker::WorkingMode::RECORD_PATH); } diff --git a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_navigate_forward.cpp b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_navigate_forward.cpp index 76d813a4c..2a5a43a74 100644 --- a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_navigate_forward.cpp +++ b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_navigate_forward.cpp @@ -68,15 +68,13 @@ void CbNavigateForward::onEntry() auto p = nav2zClient_->getComponent(); auto referenceFrame = p->getReferenceFrame(); auto currentPoseMsg = p->toPoseMsg(); - tf2::Transform currentPose; - tf2::fromMsg(currentPoseMsg, currentPose); RCLCPP_INFO_STREAM( getLogger(), "[" << getName() << "]" << "current pose: " << currentPoseMsg); // force global orientation if it is requested - if (options.forwardSpeed) + if (options.forceInitialOrientation) { currentPoseMsg.orientation = *(options.forceInitialOrientation); RCLCPP_WARN_STREAM( @@ -85,6 +83,9 @@ void CbNavigateForward::onEntry() << "Forcing initial straight motion orientation: " << currentPoseMsg.orientation); } + tf2::Transform currentPose; + tf2::fromMsg(currentPoseMsg, currentPose); + tf2::Transform targetPose; if (goalPose_) { diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/openvpn_file_run.sh b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/openvpn_file_run.sh index 0d35527a9..dc16db9e7 100755 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/openvpn_file_run.sh +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/openvpn_file_run.sh @@ -4,4 +4,4 @@ xhost + DIR="$(dirname "$(realpath "$0")")" ROOT_DIR=`realpath $DIR/../../../..` -sudo nvidia-docker run --net host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name smacc_ue -v $ROOT_DIR/SMACC2:/home/ros2_ws/src/SMACC2 -v $ROOT_DIR/SMACC2_RTA:/home/ros2_ws/src/SMACC2_RTA -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash +sudo nvidia-docker run --net host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name ue_editor_rclue -v $ROOT_DIR/SMACC2:/home/ros2_ws/src/SMACC2 -v $ROOT_DIR/SMACC2_RTA:/home/ros2_ws/src/SMACC2_RTA -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/run_docker_container_bash_development.sh b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/run_docker_container_bash_development.sh index 86493df9b..257b64e62 100755 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/run_docker_container_bash_development.sh +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/run_docker_container_bash_development.sh @@ -5,4 +5,4 @@ DIR="$(dirname "$(realpath "$0")")" ROOT_DIR=`realpath $DIR/../../../..` DRIVER=$(apt list --installed | grep nvidia-driver) -sudo nvidia-docker run --net host --ipc=host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name smacc_ue -v $ROOT_DIR/ue_project_1:/home/ros2_ws/src/ue_project_1 -v $ROOT_DIR/ue_project_2:/home/ros2_ws/src/ue_project_2 -v $ROOT_DIR/SMACC2:/home/ros2_ws/src/SMACC2 -v $ROOT_DIR/UE-Plugins:/home/ros2_ws/src/ue_project_1/Plugins/RapyutaSimulationPlugins -v $ROOT_DIR/rclUE:/home/ros2_ws/src/ue_project_1/Plugins/rclUE -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash -c "./nvidia-driver-check.sh '$DRIVER' && bash" +sudo nvidia-docker run --net host --ipc=host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name ue_editor_rclue -v $ROOT_DIR/ue_project_1:/home/ros2_ws/src/ue_project_1 -v $ROOT_DIR/ue_project_2:/home/ros2_ws/src/ue_project_2 -v $ROOT_DIR/SMACC2:/home/ros2_ws/src/SMACC2 -v $ROOT_DIR/UE-Plugins:/home/ros2_ws/src/ue_project_1/Plugins/RapyutaSimulationPlugins -v $ROOT_DIR/rclUE:/home/ros2_ws/src/ue_project_1/Plugins/rclUE -v $ROOT_DIR/UE-Plugins:/home/ros2_ws/src/ue_project_2/Plugins/RapyutaSimulationPlugins -v $ROOT_DIR/rclUE:/home/ros2_ws/src/ue_project_2/Plugins/rclUE -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash -c "./nvidia-driver-check.sh '$DRIVER' && bash" diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/sm_dancebot_artgallery_ue.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/sm_dancebot_artgallery_ue.hpp index 2541d17a1..5d5ce96a1 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/sm_dancebot_artgallery_ue.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/sm_dancebot_artgallery_ue.hpp @@ -28,6 +28,7 @@ #include #include +#include // #include #include @@ -47,7 +48,6 @@ #include #include - // ORTHOGONALS #include #include diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_forward_1.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_forward_1.hpp index 6913a3190..9c0de7c51 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_forward_1.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_forward_1.hpp @@ -48,9 +48,18 @@ struct StiFPatternForward1 : public smacc2::SmaccState, void runtimeConfigure() { - // auto &superstate = TSti::template context(); - // RCLCPP_INFO(this->getLogger(),"[SsrFpattern] Fpattern rotate: SS current iteration: %d/%d", - // superstate.iteration_count, SS::total_iterations()); + cl_nav2z::odom_tracker::CpOdomTracker * odomTracker; + this->requiresComponent(odomTracker); + auto* cbForwardMotion = this->template getOrthogonal()->template getClientBehavior(); + auto previousGoal = odomTracker->getCurrentMotionGoal(); + + if (previousGoal) + { + cbForwardMotion->options.forceInitialOrientation = previousGoal->pose.orientation; + RCLCPP_ERROR_STREAM(this->getLogger(), "Previous goal orientation: " << previousGoal->pose.orientation.x << ", " << previousGoal->pose.orientation.y << ", " << previousGoal->pose.orientation.z << ", " << previousGoal->pose.orientation.w); + }; + + RCLCPP_ERROR_STREAM(this->getLogger(), ".."); } }; } // namespace f_pattern_states diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_rotate_1.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_rotate_1.hpp index 927b54bf6..ba02636d3 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_rotate_1.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_rotate_1.hpp @@ -42,19 +42,22 @@ struct StiFPatternRotate1 : smacc2::SmaccState, SS> static void staticConfigure() { float angle = 0; - double offset = -1.5; // for a better behaving + double offset = 0; //-1.5; // for a better behaving if (SS::direction() == TDirection::LEFT) angle = 90 + offset; else angle = -90 - offset; - + //TSti::template configure_orthogonal(angle); TSti::template configure_orthogonal( angle); // absolute aligned to the y-axis } - void runtimeConfigure() {} + void runtimeConfigure() + { + + } }; } // namespace f_pattern_states } // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_rotate_2.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_rotate_2.hpp index e13928efe..8dd70ec2f 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_rotate_2.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_rotate_2.hpp @@ -41,7 +41,7 @@ struct StiFPatternRotate2 : smacc2::SmaccState, SS> // STATE FUNCTIONS static void staticConfigure() { - double offset = 1.5; // for a better behaving + double offset = 0;//1.5; // for a better behaving // float angle = 0; // if (SS::direction() == TDirection::LEFT) diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_navigate_artgallery_waypoints.x.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_navigate_artgallery_waypoints.x.hpp index e7d2a1f33..d12ea6372 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_navigate_artgallery_waypoints.x.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_navigate_artgallery_waypoints.x.hpp @@ -42,7 +42,7 @@ struct StNavigateArtGalleryWaypointsX : smacc2::SmaccState, // Transition, SS5::SsSPattern1, SUCCESS>, - Transition, SS4::SsFPattern1, SUCCESS>, + Transition, SS4::SsFPattern1, SUCCESS>, Transition, SS1::SsRadialPattern1, SUCCESS>, Transition, StNavigateArtGalleryWaypointsX, SUCCESS>, Transition, StNavigateArtGalleryWaypointsX, ABORT>, diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_f_pattern_1.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_f_pattern_1.hpp index 72f9be867..c429f53ce 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_f_pattern_1.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_f_pattern_1.hpp @@ -66,10 +66,12 @@ struct SsFPattern1 static constexpr float pitch_lenght_meters() { return 0.75; } static constexpr int total_iterations() { return 8; } static constexpr TDirection direction() { return TDirection::RIGHT; } - + // superstate state variables int iteration_count; + double last_goal_angle_rad; + // STATE FUNCTIONS static void staticConfigure() { diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/nav2_params.yaml b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/nav2_params.yaml index 8f8691cfd..88ad0d7f6 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/nav2_params.yaml +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/nav2_params.yaml @@ -120,12 +120,12 @@ controller_server: stateful: True backward_goal_checker: plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 1.3 #0.9 - yaw_goal_tolerance: 0.1 #0.05 + xy_goal_tolerance: 0.25 #0.9 + yaw_goal_tolerance: 0.05 #0.05 stateful: True forward_goal_checker: plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 1.3 #0.09 + xy_goal_tolerance: 0.08 #0.09 yaw_goal_tolerance: 0.1 #0.05 stateful: True absolute_rotate_goal_checker: @@ -135,8 +135,8 @@ controller_server: stateful: True undo_path_backwards_goal_checker: plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 0.04 # 4 cm - yaw_goal_tolerance: 0.14 + xy_goal_tolerance: 0.02 # 4 cm + yaw_goal_tolerance: 0.34 stateful: True # DWB parameters @@ -186,27 +186,28 @@ controller_server: plugin: "backward_local_planner::BackwardLocalPlanner" transform_tolerance: 0.5 k_rho: -1.4 # linear distance to the goal gain, default = -1.0 (related with carrot distance) - k_alpha: -0.1 #-0.1 - k_betta: -0.1 # -1.0 - carrot_distance: 0.3 #meters default 0.5 + k_alpha: -16.4 #-0.1 + k_betta: -0.01 # -1.0 + carrot_distance: 0.8 #meters default 0.5 carrot_angular_distance: 0.3 # no constraint, max 3.1416 pure_spinning_straight_line_mode: true max_linear_x_speed: 0.25 + max_angular_z_speed: 0.4 ForwardLocalPlanner: plugin: "cl_nav2z::forward_local_planner::ForwardLocalPlanner" transform_tolerance: 0.5 k_rho: 2.5 - k_alpha: -0.4 - k_betta: -1.0 - carrot_distance: 0.5 #meters + k_alpha: -2.4 + k_betta: -0.1 + carrot_distance: 0.8 #meters carrot_angular_distance: 0.5 # no constraint, max 3.1416 max_linear_x_speed: 0.3 - max_angular_z_speed: 0.2 + max_angular_z_speed: 0.4 PureSpinningLocalPlanner: plugin: "pure_spinning_local_planner::PureSpinningLocalPlanner" transform_tolerance: 0.5 - k_betta: 10.0 - max_angular_z_speed: 0.2 + k_betta: 25.0 + max_angular_z_speed: 0.3 use_shortest_angular_distance: true diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/waypoints_art_gallery.yaml b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/waypoints_art_gallery.yaml index 1d1555c04..2f0b75e5b 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/waypoints_art_gallery.yaml +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/waypoints_art_gallery.yaml @@ -60,15 +60,15 @@ waypoints: # z: 0.0 # w: 1.0 # POINT R8 - - position: - x: -3.70 - y: 26.70 - z: 0.0 - orientation: - x: 0.0 - y: -0.0 - z: 0.0 - w: 1.0 + # - position: + # x: -3.70 + # y: 26.70 + # z: 0.0 + # orientation: + # x: 0.0 + # y: -0.0 + # z: 0.0 + # w: 1.0 # POINT R10 - position: x: -8.0 diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/rviz/nav2_default_view.rviz b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/rviz/nav2_default_view.rviz index 6c26718d0..90b8334ce 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/rviz/nav2_default_view.rviz +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/rviz/nav2_default_view.rviz @@ -5,6 +5,7 @@ Panels: Property Tree Widget: Expanded: - /Global Options1 + - /Grid1 - /TF1 - /TF1/Frames1 - /TF1/Tree1 @@ -12,9 +13,10 @@ Panels: - /Global Planner1/Global Costmap1 - /Controller1 - /Odometry1 + - /Odometry1/Shape1 - /LaserScan1/Topic1 Splitter Ratio: 0.5833333134651184 - Tree Height: 865 + Tree Height: 685 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -47,7 +49,7 @@ Visualization Manager: Y: 0 Z: 0 Plane: XY - Plane Cell Count: 10 + Plane Cell Count: 100 Reference Frame: Value: true - Alpha: 1 @@ -558,8 +560,8 @@ Visualization Manager: Color: 255; 25; 0 Head Length: 0 Head Radius: 0 - Shaft Length: 0.02800000086426735 - Shaft Radius: 0.009999999776482582 + Shaft Length: 0.03799999877810478 + Shaft Radius: 0.019999999552965164 Value: Arrow Topic: Depth: 5 @@ -609,7 +611,7 @@ Visualization Manager: Enabled: true Name: BackwardLocalPlannerGoalMarker Namespaces: - {} + my_namespace2: true Topic: Depth: 5 Durability Policy: Volatile @@ -633,7 +635,7 @@ Visualization Manager: Enabled: true Name: UndoGlobalPlannerMarkers Namespaces: - {} + my_namespace2: true Topic: Depth: 5 Durability Policy: Volatile @@ -768,7 +770,7 @@ Visualization Manager: Value: true Views: Current: - Angle: -1.7249938249588013 + Angle: -1.5699937343597412 Class: rviz_default_plugins/TopDownOrtho Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 @@ -778,21 +780,21 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Scale: -175.2274627685547 + Scale: -185.1104278564453 Target Frame: base_link Value: TopDownOrtho (rviz_default_plugins) - X: -0.12203919142484665 - Y: 0.4557923972606659 + X: 0.5208465456962585 + Y: 0.037297897040843964 Saved: ~ Window Geometry: Displays: collapsed: true - Height: 1016 + Height: 836 Hide Left Dock: true Hide Right Dock: true Navigation 2: collapsed: false - QMainWindow State: 000000ff00000000fd0000000400000000000002860000039efc020000000bfb0000001200530065006c0065006300740069006f006e000000003d000000830000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003200000002da000001010000013900fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000002800fffffffb0000001e005200650061006c00730065006e0073006500430061006d0065007200610100000177000000fc0000000000000000000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000039d0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd000000040000000000000286000002eafc020000000bfb0000001200530065006c0065006300740069006f006e000000003d000000830000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d000002ea000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003200000002da000001010000013900fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000002800fffffffb0000001e005200650061006c00730065006e0073006500430061006d0065007200610100000177000000fc0000000000000000000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000039d000002ea00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 RealsenseCamera: collapsed: false Selection: diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/attach_docker_container.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/attach_docker_container.sh index 37343f136..7842a41d4 100755 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/attach_docker_container.sh +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/attach_docker_container.sh @@ -1 +1 @@ -sudo docker attach smacc_ue +sudo docker attach ue_editor_rclue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/join_bash.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/join_bash.sh index bc3ad61e3..29ee10381 100755 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/join_bash.sh +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/join_bash.sh @@ -1 +1 @@ -sudo docker exec -it smacc_ue /bin/bash +sudo docker exec -it ue_editor_rclue /bin/bash diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/join_editor.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/join_editor.sh index d215b6054..c11f2028e 100755 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/join_editor.sh +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/join_editor.sh @@ -1 +1 @@ -sudo docker exec -it smacc_ue ./run_editor_smacc.sh +sudo docker exec -it ue_editor_rclue ./run_editor_smacc.sh diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/openvpn_file_run.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/openvpn_file_run.sh index 0d35527a9..dc16db9e7 100755 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/openvpn_file_run.sh +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/openvpn_file_run.sh @@ -4,4 +4,4 @@ xhost + DIR="$(dirname "$(realpath "$0")")" ROOT_DIR=`realpath $DIR/../../../..` -sudo nvidia-docker run --net host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name smacc_ue -v $ROOT_DIR/SMACC2:/home/ros2_ws/src/SMACC2 -v $ROOT_DIR/SMACC2_RTA:/home/ros2_ws/src/SMACC2_RTA -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash +sudo nvidia-docker run --net host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name ue_editor_rclue -v $ROOT_DIR/SMACC2:/home/ros2_ws/src/SMACC2 -v $ROOT_DIR/SMACC2_RTA:/home/ros2_ws/src/SMACC2_RTA -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/remove_container.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/remove_container.sh index e3880a51d..3b98c52e0 100755 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/remove_container.sh +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/remove_container.sh @@ -1,2 +1,2 @@ -sudo docker stop smacc_ue -sudo docker rm smacc_ue +sudo docker stop ue_editor_rclue +sudo docker rm ue_editor_rclue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_bash.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_bash.sh index 5ab294c08..a450ca485 100755 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_bash.sh +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_bash.sh @@ -5,4 +5,4 @@ DIR="$(dirname "$(realpath "$0")")" ROOT_DIR=`realpath $DIR/../../../..` DRIVER=$(apt list --installed | grep nvidia-driver) -sudo nvidia-docker run --net host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name smacc_ue -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash -c "./nvidia-driver-check.sh '$DRIVER' && bash" +sudo nvidia-docker run --net host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name ue_editor_rclue -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash -c "./nvidia-driver-check.sh '$DRIVER' && bash" diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_bash_development.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_bash_development.sh index 1ee8c6830..f33ee1e08 100755 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_bash_development.sh +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_bash_development.sh @@ -5,4 +5,4 @@ DIR="$(dirname "$(realpath "$0")")" ROOT_DIR=`realpath $DIR/../../../..` DRIVER=$(apt list --installed | grep nvidia-driver) -sudo nvidia-docker run --net host --ipc=host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name smacc_ue -v $ROOT_DIR/ue_project_1:/home/ros2_ws/src/ue_project_1 -v $ROOT_DIR/SMACC2:/home/ros2_ws/src/SMACC2 -v $ROOT_DIR/UE-Plugins:/home/ros2_ws/src/ue_project_1/Plugins/RapyutaSimulationPlugins -v $ROOT_DIR/rclUE:/home/ros2_ws/src/ue_project_1/Plugins/rclUE -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash -c "./nvidia-driver-check.sh '$DRIVER' && bash" +sudo nvidia-docker run --net host --ipc=host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name ue_editor_rclue -v $ROOT_DIR/ue_project_1:/home/ros2_ws/src/ue_project_1 -v $ROOT_DIR/SMACC2:/home/ros2_ws/src/SMACC2 -v $ROOT_DIR/UE-Plugins:/home/ros2_ws/src/ue_project_1/Plugins/RapyutaSimulationPlugins -v $ROOT_DIR/rclUE:/home/ros2_ws/src/ue_project_1/Plugins/rclUE -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash -c "./nvidia-driver-check.sh '$DRIVER' && bash" diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_editor.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_editor.sh index 2e93b8c7e..8dfad38c4 100755 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_editor.sh +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_editor.sh @@ -5,4 +5,4 @@ DIR="$(dirname "$(realpath "$0")")" ROOT_DIR=`realpath $DIR/../../../..` DRIVER=$(apt list --installed | grep nvidia-driver) -sudo nvidia-docker run --net host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name smacc_ue -v $ROOT_DIR/ue_project_2:/home/ros2_ws/src/ue_project_2 -v $ROOT_DIR/ue_project_1:/home/ros2_ws/src/ue_project_1 -v $ROOT_DIR/SMACC2:/home/ros2_ws/src/SMACC2 -v $ROOT_DIR/UE-Plugins:/home/ros2_ws/src/turtlebot3-UE/Plugins/RapyutaSimulationPlugins -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash -c "./nvidia-driver-check.sh '$DRIVER' && ./run_editor_smacc.sh" +sudo nvidia-docker run --net host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name ue_editor_rclue -v $ROOT_DIR/ue_project_2:/home/ros2_ws/src/ue_project_2 -v $ROOT_DIR/ue_project_1:/home/ros2_ws/src/ue_project_1 -v $ROOT_DIR/SMACC2:/home/ros2_ws/src/SMACC2 -v $ROOT_DIR/UE-Plugins:/home/ros2_ws/src/turtlebot3-UE/Plugins/RapyutaSimulationPlugins -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash -c "./nvidia-driver-check.sh '$DRIVER' && ./run_editor_smacc.sh" diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/start_container.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/start_container.sh index ec24e9d12..6b339f906 100755 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/start_container.sh +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/start_container.sh @@ -1 +1 @@ -sudo docker start smacc_ue +sudo docker start ue_editor_rclue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/stop_container.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/stop_container.sh index 3290cdb0c..0c06686cc 100755 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/stop_container.sh +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/stop_container.sh @@ -1 +1 @@ -sudo docker stop smacc_ue +sudo docker stop ue_editor_rclue From d24b60b76b37f4a4a7cad39c0b7be090ba679250 Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Wed, 16 Aug 2023 08:33:53 +0200 Subject: [PATCH 62/70] missing --- .../st_navigate_artgallery_waypoints.x.hpp | 6 +++--- .../superstates/ss_radial_pattern_1.hpp | 2 +- .../nav2z_client/waypoints_art_gallery.yaml | 18 +++++++++--------- 3 files changed, 13 insertions(+), 13 deletions(-) diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_navigate_artgallery_waypoints.x.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_navigate_artgallery_waypoints.x.hpp index d12ea6372..ac7aa90b5 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_navigate_artgallery_waypoints.x.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_navigate_artgallery_waypoints.x.hpp @@ -41,9 +41,9 @@ struct StNavigateArtGalleryWaypointsX : smacc2::SmaccState, - // Transition, SS5::SsSPattern1, SUCCESS>, - Transition, SS4::SsFPattern1, SUCCESS>, - Transition, SS1::SsRadialPattern1, SUCCESS>, + Transition, SS5::SsSPattern1, SUCCESS>, + Transition, SS4::SsFPattern1, SUCCESS>, + Transition, SS1::SsRadialPattern1, SUCCESS>, Transition, StNavigateArtGalleryWaypointsX, SUCCESS>, Transition, StNavigateArtGalleryWaypointsX, ABORT>, Transition, StNavigateArtGalleryWaypointsX, ABORT> diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_radial_pattern_1.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_radial_pattern_1.hpp index 40826e546..9bb9c920e 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_radial_pattern_1.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_radial_pattern_1.hpp @@ -56,7 +56,7 @@ struct SsRadialPattern1 static constexpr int total_iterations() { return 16; } static constexpr float ray_angle_increment_degree() { return 360.0 / total_iterations(); } - static constexpr float ray_length_meters() { return 4.0; } + static constexpr float ray_length_meters() { return 3.0; } int iteration_count = 0; diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/waypoints_art_gallery.yaml b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/waypoints_art_gallery.yaml index 2f0b75e5b..1d1555c04 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/waypoints_art_gallery.yaml +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/waypoints_art_gallery.yaml @@ -60,15 +60,15 @@ waypoints: # z: 0.0 # w: 1.0 # POINT R8 - # - position: - # x: -3.70 - # y: 26.70 - # z: 0.0 - # orientation: - # x: 0.0 - # y: -0.0 - # z: 0.0 - # w: 1.0 + - position: + x: -3.70 + y: 26.70 + z: 0.0 + orientation: + x: 0.0 + y: -0.0 + z: 0.0 + w: 1.0 # POINT R10 - position: x: -8.0 From 25fb00cd225018449da94ed65a02b06f5c91230f Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Wed, 16 Aug 2023 18:20:02 +0200 Subject: [PATCH 63/70] art gallery --- .../cb_nav2z_client_behavior_base.cpp | 3 +++ .../st_navigate_artgallery_waypoints.x.hpp | 4 ++-- .../params/nav2z_client/nav2_params.yaml | 4 ++-- .../rviz/nav2_default_view.rviz | 18 +++++++++--------- 4 files changed, 16 insertions(+), 13 deletions(-) diff --git a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_nav2z_client_behavior_base.cpp b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_nav2z_client_behavior_base.cpp index 6794e7bf5..31838233b 100644 --- a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_nav2z_client_behavior_base.cpp +++ b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_nav2z_client_behavior_base.cpp @@ -80,6 +80,9 @@ bool CbNav2ZClientBehaviorBase::isOwnActionResponse(const ClNav2Z::WrappedResult void CbNav2ZClientBehaviorBase::onNavigationResult(const ClNav2Z::WrappedResult & r) { + RCLCPP_DEBUG( + getLogger(), "[%s] Received result event from action server, result code", getName().c_str()); + if (r.code == rclcpp_action::ResultCode::SUCCEEDED) { this->onNavigationActionSuccess(r); diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_navigate_artgallery_waypoints.x.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_navigate_artgallery_waypoints.x.hpp index ac7aa90b5..527fa4618 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_navigate_artgallery_waypoints.x.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_navigate_artgallery_waypoints.x.hpp @@ -42,8 +42,8 @@ struct StNavigateArtGalleryWaypointsX : smacc2::SmaccState, Transition, SS5::SsSPattern1, SUCCESS>, - Transition, SS4::SsFPattern1, SUCCESS>, - Transition, SS1::SsRadialPattern1, SUCCESS>, + Transition, SS4::SsFPattern1, SUCCESS>, + Transition, SS1::SsRadialPattern1, SUCCESS>, Transition, StNavigateArtGalleryWaypointsX, SUCCESS>, Transition, StNavigateArtGalleryWaypointsX, ABORT>, Transition, StNavigateArtGalleryWaypointsX, ABORT> diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/nav2_params.yaml b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/nav2_params.yaml index 88ad0d7f6..21f3cf2bf 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/nav2_params.yaml +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/nav2_params.yaml @@ -116,7 +116,7 @@ controller_server: goal_checker: plugin: "nav2_controller::SimpleGoalChecker" xy_goal_tolerance: 0.257 - yaw_goal_tolerance: 1.65 # almost free orientation + yaw_goal_tolerance: 0.1 # 1.65 # almost free orientation stateful: True backward_goal_checker: plugin: "nav2_controller::SimpleGoalChecker" @@ -131,7 +131,7 @@ controller_server: absolute_rotate_goal_checker: plugin: "nav2_controller::SimpleGoalChecker" xy_goal_tolerance: 0.15 - yaw_goal_tolerance: 0.01 #0.005 + yaw_goal_tolerance: 0.015 #0.005 stateful: True undo_path_backwards_goal_checker: plugin: "nav2_controller::SimpleGoalChecker" diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/rviz/nav2_default_view.rviz b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/rviz/nav2_default_view.rviz index 90b8334ce..b4582e7a6 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/rviz/nav2_default_view.rviz +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/rviz/nav2_default_view.rviz @@ -16,7 +16,7 @@ Panels: - /Odometry1/Shape1 - /LaserScan1/Topic1 Splitter Ratio: 0.5833333134651184 - Tree Height: 685 + Tree Height: 441 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -561,7 +561,7 @@ Visualization Manager: Head Length: 0 Head Radius: 0 Shaft Length: 0.03799999877810478 - Shaft Radius: 0.019999999552965164 + Shaft Radius: 0.03999999910593033 Value: Arrow Topic: Depth: 5 @@ -611,7 +611,7 @@ Visualization Manager: Enabled: true Name: BackwardLocalPlannerGoalMarker Namespaces: - my_namespace2: true + {} Topic: Depth: 5 Durability Policy: Volatile @@ -623,7 +623,7 @@ Visualization Manager: Enabled: true Name: ForwardCarrot Namespaces: - my_namespace2: true + {} Topic: Depth: 5 Durability Policy: Volatile @@ -635,7 +635,7 @@ Visualization Manager: Enabled: true Name: UndoGlobalPlannerMarkers Namespaces: - my_namespace2: true + {} Topic: Depth: 5 Durability Policy: Volatile @@ -780,7 +780,7 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Scale: -185.1104278564453 + Scale: -49.22471237182617 Target Frame: base_link Value: TopDownOrtho (rviz_default_plugins) X: 0.5208465456962585 @@ -794,7 +794,7 @@ Window Geometry: Hide Right Dock: true Navigation 2: collapsed: false - QMainWindow State: 000000ff00000000fd000000040000000000000286000002eafc020000000bfb0000001200530065006c0065006300740069006f006e000000003d000000830000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d000002ea000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003200000002da000001010000013900fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000002800fffffffb0000001e005200650061006c00730065006e0073006500430061006d0065007200610100000177000000fc0000000000000000000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000039d000002ea00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd000000040000000000000286000002eafc020000000bfb0000001200530065006c0065006300740069006f006e000000003d000000830000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d000002ea000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003200000002da000001010000013900fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000002800fffffffb0000001e005200650061006c00730065006e0073006500430061006d0065007200610100000177000000fc0000000000000000000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000039d0000031100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000 RealsenseCamera: collapsed: false Selection: @@ -804,5 +804,5 @@ Window Geometry: Views: collapsed: true Width: 925 - X: 70 - Y: 27 + X: 85 + Y: 28 From c355b55f9bf5ecf7ffe5ee5bbe0d86e936f4a63a Mon Sep 17 00:00:00 2001 From: brettpac Date: Thu, 17 Aug 2023 08:37:15 -0700 Subject: [PATCH 64/70] Update README.md --- smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md | 5 ----- 1 file changed, 5 deletions(-) diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md b/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md index 30eb2c178..a83fd099b 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md @@ -7,11 +7,6 @@ Here are some important notes regarding the solution: **Repos Versioning:** Not all versions of `rclUE`, `turtlebot3Editor`, and `RapyputaPlugins` are compatible with each other. Ensuring consistency among them can be challenging, especially when considering that the container maps/volumes some volumes with external contents. The container is already designed to have a correct combination of all of them, but this is hardcoded in the Dockerfile and can be improved. -**DDS Configuration:** There may be communication issues between the Docker container nodes and host nodes. As a workaround, we currently use `cyclonedds` on the host (until a uniform solution using `fastrtps` is found). To set this up, add the following line to the `.bashrc` file on the host: -``` -export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp -``` - ***Automatic container nvidia driver update*** The host and the container must have the same nvidia-driver in order to run the ue editor and simulation. There is a mechanism implemented to automatically sync the driver. The current driver version is passed from the host to the container and then it is updated in the container if that is required. That is done in the nvidia-check.sh script. From 109440de493ea4e1ae671ab214d4e3b3b33fee3b Mon Sep 17 00:00:00 2001 From: brettpac Date: Thu, 17 Aug 2023 09:20:36 -0700 Subject: [PATCH 65/70] Update README.md --- .../sm_dancebot_ue/docker/README.md | 52 +++++++------------ 1 file changed, 18 insertions(+), 34 deletions(-) diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md b/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md index a83fd099b..6986dc9a9 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md @@ -128,67 +128,50 @@ This section explains how to run and create a new container from the `ue_editor_ To run the Unreal Editor inside the container, you need to use some auxiliary scripts located in the `sm_dancebot_ue` example. Follow these steps: -1. Download the current SMACC2 repository. +1. *Requirements* To run the docker container you need to download a few folders that will be mounted inside the container. These are: rclUE, UE-Plugins, SMACC2, ue_msgs, ue_poroject_1,... ue_project_n + +![image](https://github.com/robosoft-ai/SMACC2/assets/9130104/6151fac0-f0ba-4dd3-81fa-9bf1ca899142) -2. Navigate to the `sm_dancebot_ue/docker` folder. -3. Execute the following command: - ``` - ./run_docker_container_editor.sh +3. Navigate to the `sm_dancebot_ue/docker` folder. + +4. Execute the following command: ``` + ./run_docker_container_bash_development.sh ``` This will run and create a new container using the `ue_editor_rcl` Docker image. -4. The Unreal Engine editor will automatically open in "edition mode." You can launch the simulation with Turtlebot topics accessible from both the container and the host computer by clicking the "play" button. +5. The Unreal Engine editor will automatically open in "edition mode." You can launch the simulation with Turtlebot topics accessible from both the container and the host computer by clicking the "play" button. Note: When you close the editor, the container will also be finished, but it will remain installed. You can reopen the editor using the command: ``` ./start_container.sh ``` -### (Alternative) Running/Creating a New Container for Container Debugging - -There is an alternative way to create the container in daemon mode, where the lifetime of the container is not tied to the Unreal Editor window. This is useful, especially for developing new features for the container. To create the container in this mode,: +### Joining, Stopping and Removing existing container -1. Execute the following command: - ``` - ./run_docker_container_bash.sh - ``` - - This will create and start a new container as a daemon. The container will be available even after restarting. It is capable of opening the Unreal Engine editor with ROS2, but the editor will not open automatically. - -2. Execute the editor proccess: - ``` - ./join_editor.sh - ``` +1. To enter the Docker container and debug or test things from the command line, use the following command: -The Unreal Engine editor will automatically open in "edition mode." You can launch the simulation with Turtlebot topics accessible from both the container and the host computer by clicking the "play" button. - -### Stopping and Removing a Running Container +``` +./join_bash.sh +``` If you need to reset everything and remove the existing container, follow these steps: -1. Execute the following command: +2. Execute the following command: ``` ./stop_container.sh ``` This will stop the running container. -2. Execute the following command: +3. Execute the following command: ``` ./remove_container.sh ``` This will remove the existing container. -### Joining the Container via Bash - -To enter the Docker container and debug or test things from the command line, use the following command: - -``` -./join_bash.sh -``` ### Connecting the Container to VPN @@ -245,8 +228,9 @@ You'll need to open three terminals for this demo. ``` cd ~/workspace/humble_ws/ source /opt/ros/humble/setup.bash - colcon build - + colcon build --symlink-install --parallel-workers 4 + # optionally, to go faster to our project: --packages-up-to sm_dancebot_ue + # optionally, to build debug mode: --cmake-args -DCMAKE_BUILD_TYPE=Debug ``` Once everything is done building... 3. Navigate to the `sm_dancebot_ue/docker` folder. From 7567f4d501ec975ce12f331d026d1291e284e0fb Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Fri, 18 Aug 2023 17:27:23 +0200 Subject: [PATCH 66/70] fixes --- .../sti_spattern_forward_4.hpp | 14 ++++-- .../sti_spattern_rotate_1.hpp | 39 ++++++++++----- .../sti_spattern_rotate_3.hpp | 49 ++++++++++++------- .../sti_spattern_rotate_4.hpp | 29 +++++++---- .../launch/sm_dancebot_ue_launch.py | 2 +- 5 files changed, 87 insertions(+), 46 deletions(-) diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_forward_4.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_forward_4.hpp index d6736d5a9..ab3ce3d71 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_forward_4.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_forward_4.hpp @@ -36,15 +36,19 @@ struct StiSPatternForward4 : public smacc2::SmaccState >reactions; // STATE FUNCTIONS - static void staticConfigure() {} - - void runtimeConfigure() + static void staticConfigure() { - //auto &superstate = this->context(); - this->configure(SS::pitch2_lenght_meters()); this->configure(); } + + // void runtimeConfigure() + // { + // //auto &superstate = this->context(); + + // this->configure(SS::pitch2_lenght_meters()); + // this->configure(); + // } }; } // namespace s_pattern_states } // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_1.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_1.hpp index ec045e1a4..584f970dd 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_1.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_1.hpp @@ -36,23 +36,38 @@ struct StiSPatternRotate1 : smacc2::SmaccState >reactions; // STATE FUNCTIONS - static void staticConfigure() {} + static void staticConfigure() + { + float offset = 0; + float angle = 0; + if (SS::direction() == TDirection::LEFT) + angle = 180 + offset; + else + angle = 0 - offset; + + configure_orthogonal(angle); + configure_orthogonal(); + } void runtimeConfigure() { auto & superstate = this->context(); + RCLCPP_INFO( + getLogger(), "[StiSPatternRotate] SpatternRotate rotate: SS current iteration: %d/%d", + superstate.iteration_count, SS::total_iterations()); + // auto & superstate = this->context(); - float offset = 0; - if (superstate.direction() == TDirection::RIGHT) - { - // - offset because we are looking to the north and we have to turn clockwise - this->configure(0 - offset); - } - else - { - // - offset because we are looking to the south and we have to turn counter-clockwise - this->configure(180 + offset); - } + // float offset = 0; + // if (superstate.direction() == TDirection::RIGHT) + // { + // // - offset because we are looking to the north and we have to turn clockwise + // this->configure(0 - offset); + // } + // else + // { + // // - offset because we are looking to the south and we have to turn counter-clockwise + // this->configure(180 + offset); + // } } }; } // namespace s_pattern_states diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_3.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_3.hpp index 70680cf33..3ef30b966 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_3.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_3.hpp @@ -36,7 +36,18 @@ struct StiSPatternRotate3 : smacc2::SmaccState >reactions; // STATE FUNCTIONS - static void staticConfigure() {} + static void staticConfigure() + { + float offset = 0; + float angle = 0; + if (SS::direction() == TDirection::LEFT) + angle = 180 + offset; + else + angle = 0 - offset; + + configure_orthogonal(angle); + configure_orthogonal(); + } void runtimeConfigure() { @@ -45,27 +56,27 @@ struct StiSPatternRotate3 : smacc2::SmaccState getLogger(), "[StiSPatternRotate] SpatternRotate rotate: SS current iteration: %d/%d", superstate.iteration_count, SS::total_iterations()); - float offset = 0; - // float angle = 0; - // if (superstate.direction() == TDirection::LEFT) - // angle = -90 - offset; - // else - // angle = +90 + offset; + // float offset = 0; + // // float angle = 0; + // // if (superstate.direction() == TDirection::LEFT) + // // angle = -90 - offset; + // // else + // // angle = +90 + offset; - // this->configure(angle); + // // this->configure(angle); - if (superstate.direction() == TDirection::RIGHT) - { - // - offset because we are looking to the north and we have to turn clockwise - this->configure(0 - offset); - } - else - { - // - offset because we are looking to the south and we have to turn counter-clockwise - this->configure(180 + offset); - } + // if (superstate.direction() == TDirection::RIGHT) + // { + // // - offset because we are looking to the north and we have to turn clockwise + // this->configure(0 - offset); + // } + // else + // { + // // - offset because we are looking to the south and we have to turn counter-clockwise + // this->configure(180 + offset); + // } - this->configure(); + // this->configure(); } }; } // namespace s_pattern_states diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_4.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_4.hpp index e447f75c7..b49f7675a 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_4.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_4.hpp @@ -36,7 +36,18 @@ struct StiSPatternRotate4 : smacc2::SmaccState >reactions; // STATE FUNCTIONS - static void staticConfigure() {} + static void staticConfigure() + { + float offset = 0; + float angle = 0; + if (SS::direction() == TDirection::LEFT) + angle = -90 - offset; + else + angle = 90 + offset; + + configure_orthogonal(angle); + configure_orthogonal(); + } void runtimeConfigure() { @@ -45,15 +56,15 @@ struct StiSPatternRotate4 : smacc2::SmaccState getLogger(), "[SsrSPatternRotate] SpatternRotate rotate: SS current iteration: %d/%d", superstate.iteration_count, SS::total_iterations()); - float offset = 0; - float angle = 0; - if (superstate.direction() == TDirection::LEFT) - angle = -90 - offset; - else - angle = 90 + offset; + // float offset = 0; + // float angle = 0; + // if (superstate.direction() == TDirection::LEFT) + // angle = -90 - offset; + // else + // angle = 90 + offset; - this->configure(angle); - this->configure(); + // this->configure(angle); + // this->configure(); } }; } // namespace s_pattern_states diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/launch/sm_dancebot_ue_launch.py b/smacc2_sm_reference_library/sm_dancebot_ue/launch/sm_dancebot_ue_launch.py index 3ac471f47..ec7d28d4e 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/launch/sm_dancebot_ue_launch.py +++ b/smacc2_sm_reference_library/sm_dancebot_ue/launch/sm_dancebot_ue_launch.py @@ -196,7 +196,7 @@ def generate_launch_description(): parameters=[ os.path.join( get_package_share_directory("sm_dancebot_ue"), - "params/sm_dance_bot_config.yaml", + "params/sm_dancebot_ue_config.yaml", ) ], remappings=[ From 13340391e03ac2458fab1aea6e3644e8cf6c8673 Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Fri, 18 Aug 2023 17:28:13 +0200 Subject: [PATCH 67/70] merge --- .../sm_dancebot_ue/docker/README.md | 57 ++++++------------- 1 file changed, 18 insertions(+), 39 deletions(-) diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md b/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md index 30eb2c178..6986dc9a9 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md @@ -7,11 +7,6 @@ Here are some important notes regarding the solution: **Repos Versioning:** Not all versions of `rclUE`, `turtlebot3Editor`, and `RapyputaPlugins` are compatible with each other. Ensuring consistency among them can be challenging, especially when considering that the container maps/volumes some volumes with external contents. The container is already designed to have a correct combination of all of them, but this is hardcoded in the Dockerfile and can be improved. -**DDS Configuration:** There may be communication issues between the Docker container nodes and host nodes. As a workaround, we currently use `cyclonedds` on the host (until a uniform solution using `fastrtps` is found). To set this up, add the following line to the `.bashrc` file on the host: -``` -export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp -``` - ***Automatic container nvidia driver update*** The host and the container must have the same nvidia-driver in order to run the ue editor and simulation. There is a mechanism implemented to automatically sync the driver. The current driver version is passed from the host to the container and then it is updated in the container if that is required. That is done in the nvidia-check.sh script. @@ -133,67 +128,50 @@ This section explains how to run and create a new container from the `ue_editor_ To run the Unreal Editor inside the container, you need to use some auxiliary scripts located in the `sm_dancebot_ue` example. Follow these steps: -1. Download the current SMACC2 repository. +1. *Requirements* To run the docker container you need to download a few folders that will be mounted inside the container. These are: rclUE, UE-Plugins, SMACC2, ue_msgs, ue_poroject_1,... ue_project_n + +![image](https://github.com/robosoft-ai/SMACC2/assets/9130104/6151fac0-f0ba-4dd3-81fa-9bf1ca899142) -2. Navigate to the `sm_dancebot_ue/docker` folder. -3. Execute the following command: - ``` - ./run_docker_container_editor.sh +3. Navigate to the `sm_dancebot_ue/docker` folder. + +4. Execute the following command: ``` + ./run_docker_container_bash_development.sh ``` This will run and create a new container using the `ue_editor_rcl` Docker image. -4. The Unreal Engine editor will automatically open in "edition mode." You can launch the simulation with Turtlebot topics accessible from both the container and the host computer by clicking the "play" button. +5. The Unreal Engine editor will automatically open in "edition mode." You can launch the simulation with Turtlebot topics accessible from both the container and the host computer by clicking the "play" button. Note: When you close the editor, the container will also be finished, but it will remain installed. You can reopen the editor using the command: ``` ./start_container.sh ``` -### (Alternative) Running/Creating a New Container for Container Debugging - -There is an alternative way to create the container in daemon mode, where the lifetime of the container is not tied to the Unreal Editor window. This is useful, especially for developing new features for the container. To create the container in this mode,: - -1. Execute the following command: - ``` - ./run_docker_container_bash.sh - ``` - - This will create and start a new container as a daemon. The container will be available even after restarting. It is capable of opening the Unreal Engine editor with ROS2, but the editor will not open automatically. - -2. Execute the editor proccess: - ``` - ./join_editor.sh - ``` +### Joining, Stopping and Removing existing container -The Unreal Engine editor will automatically open in "edition mode." You can launch the simulation with Turtlebot topics accessible from both the container and the host computer by clicking the "play" button. +1. To enter the Docker container and debug or test things from the command line, use the following command: -### Stopping and Removing a Running Container +``` +./join_bash.sh +``` If you need to reset everything and remove the existing container, follow these steps: -1. Execute the following command: +2. Execute the following command: ``` ./stop_container.sh ``` This will stop the running container. -2. Execute the following command: +3. Execute the following command: ``` ./remove_container.sh ``` This will remove the existing container. -### Joining the Container via Bash - -To enter the Docker container and debug or test things from the command line, use the following command: - -``` -./join_bash.sh -``` ### Connecting the Container to VPN @@ -250,8 +228,9 @@ You'll need to open three terminals for this demo. ``` cd ~/workspace/humble_ws/ source /opt/ros/humble/setup.bash - colcon build - + colcon build --symlink-install --parallel-workers 4 + # optionally, to go faster to our project: --packages-up-to sm_dancebot_ue + # optionally, to build debug mode: --cmake-args -DCMAKE_BUILD_TYPE=Debug ``` Once everything is done building... 3. Navigate to the `sm_dancebot_ue/docker` folder. From 49e7b2a95873f03adf75d77e16722cfc65967021 Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Fri, 18 Aug 2023 18:29:55 +0200 Subject: [PATCH 68/70] missing code --- .../sti_fpattern_forward_2.hpp | 16 +++++++- .../sti_radial_end_point.hpp | 16 +++++++- .../sti_spattern_forward_4.hpp | 15 ++++---- .../sti_spattern_rotate_1.hpp | 32 +++++----------- .../sti_spattern_rotate_3.hpp | 37 ++++++------------- .../sti_spattern_rotate_4.hpp | 12 +----- 6 files changed, 59 insertions(+), 69 deletions(-) diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_forward_2.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_forward_2.hpp index 90be73344..01d9abb52 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_forward_2.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_forward_2.hpp @@ -45,7 +45,21 @@ struct StiFPatternForward2 : smacc2::SmaccState, SS> TSti::template configure_orthogonal(); } - void runtimeConfigure() {} + void runtimeConfigure() + { + cl_nav2z::odom_tracker::CpOdomTracker * odomTracker; + this->requiresComponent(odomTracker); + auto* cbForwardMotion = this->template getOrthogonal()->template getClientBehavior(); + auto previousGoal = odomTracker->getCurrentMotionGoal(); + + if (previousGoal) + { + cbForwardMotion->options.forceInitialOrientation = previousGoal->pose.orientation; + RCLCPP_ERROR_STREAM(this->getLogger(), "Previous goal orientation: " << previousGoal->pose.orientation.x << ", " << previousGoal->pose.orientation.y << ", " << previousGoal->pose.orientation.z << ", " << previousGoal->pose.orientation.w); + }; + + RCLCPP_ERROR_STREAM(this->getLogger(), ".."); + } }; } // namespace f_pattern_states } // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/radial_motion_states/sti_radial_end_point.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/radial_motion_states/sti_radial_end_point.hpp index 73af3a043..3d4dde52f 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/radial_motion_states/sti_radial_end_point.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/radial_motion_states/sti_radial_end_point.hpp @@ -43,7 +43,21 @@ struct StiRadialEndPoint : smacc2::SmaccState configure_orthogonal(); } - void runtimeConfigure() {} + void runtimeConfigure() + { + cl_nav2z::odom_tracker::CpOdomTracker * odomTracker; + this->requiresComponent(odomTracker); + auto* cbForwardMotion = this->template getOrthogonal()->template getClientBehavior(); + auto previousGoal = odomTracker->getCurrentMotionGoal(); + + if (previousGoal) + { + cbForwardMotion->options.forceInitialOrientation = previousGoal->pose.orientation; + RCLCPP_ERROR_STREAM(this->getLogger(), "Previous goal orientation: " << previousGoal->pose.orientation.x << ", " << previousGoal->pose.orientation.y << ", " << previousGoal->pose.orientation.z << ", " << previousGoal->pose.orientation.w); + }; + + RCLCPP_ERROR_STREAM(this->getLogger(), ".."); + } }; } // namespace radial_motion_states } // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_forward_4.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_forward_4.hpp index ab3ce3d71..64aaed8da 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_forward_4.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_forward_4.hpp @@ -38,17 +38,16 @@ struct StiSPatternForward4 : public smacc2::SmaccState // STATE FUNCTIONS static void staticConfigure() { - this->configure(SS::pitch2_lenght_meters()); - this->configure(); + + configure_orthogonal(SS::pitch2_lenght_meters()); + configure_orthogonal(); } - // void runtimeConfigure() - // { - // //auto &superstate = this->context(); + void runtimeConfigure() + { + //auto &superstate = this->context(); - // this->configure(SS::pitch2_lenght_meters()); - // this->configure(); - // } + } }; } // namespace s_pattern_states } // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_1.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_1.hpp index 584f970dd..45c1df93e 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_1.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_1.hpp @@ -39,35 +39,21 @@ struct StiSPatternRotate1 : smacc2::SmaccState static void staticConfigure() { float offset = 0; - float angle = 0; - if (SS::direction() == TDirection::LEFT) - angle = 180 + offset; + if (SS::direction() == TDirection::RIGHT) + { + // - offset because we are looking to the north and we have to turn clockwise + configure_orthogonal(0 - offset); + } else - angle = 0 - offset; - - configure_orthogonal(angle); - configure_orthogonal(); + { + // - offset because we are looking to the south and we have to turn counter-clockwise + configure_orthogonal(180 + offset); + } } void runtimeConfigure() { - auto & superstate = this->context(); - RCLCPP_INFO( - getLogger(), "[StiSPatternRotate] SpatternRotate rotate: SS current iteration: %d/%d", - superstate.iteration_count, SS::total_iterations()); - // auto & superstate = this->context(); - // float offset = 0; - // if (superstate.direction() == TDirection::RIGHT) - // { - // // - offset because we are looking to the north and we have to turn clockwise - // this->configure(0 - offset); - // } - // else - // { - // // - offset because we are looking to the south and we have to turn counter-clockwise - // this->configure(180 + offset); - // } } }; } // namespace s_pattern_states diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_3.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_3.hpp index 3ef30b966..20516030c 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_3.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_3.hpp @@ -39,13 +39,18 @@ struct StiSPatternRotate3 : smacc2::SmaccState static void staticConfigure() { float offset = 0; - float angle = 0; - if (SS::direction() == TDirection::LEFT) - angle = 180 + offset; + + if (SS::direction() == TDirection::RIGHT) + { + // - offset because we are looking to the north and we have to turn clockwise + configure_orthogonal(0 - offset); + } else - angle = 0 - offset; + { + // - offset because we are looking to the south and we have to turn counter-clockwise + configure_orthogonal(180 + offset); + } - configure_orthogonal(angle); configure_orthogonal(); } @@ -56,27 +61,7 @@ struct StiSPatternRotate3 : smacc2::SmaccState getLogger(), "[StiSPatternRotate] SpatternRotate rotate: SS current iteration: %d/%d", superstate.iteration_count, SS::total_iterations()); - // float offset = 0; - // // float angle = 0; - // // if (superstate.direction() == TDirection::LEFT) - // // angle = -90 - offset; - // // else - // // angle = +90 + offset; - - // // this->configure(angle); - - // if (superstate.direction() == TDirection::RIGHT) - // { - // // - offset because we are looking to the north and we have to turn clockwise - // this->configure(0 - offset); - // } - // else - // { - // // - offset because we are looking to the south and we have to turn counter-clockwise - // this->configure(180 + offset); - // } - - // this->configure(); + } }; } // namespace s_pattern_states diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_4.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_4.hpp index b49f7675a..e34959977 100644 --- a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_4.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_4.hpp @@ -51,20 +51,12 @@ struct StiSPatternRotate4 : smacc2::SmaccState void runtimeConfigure() { + auto & superstate = this->context(); RCLCPP_INFO( getLogger(), "[SsrSPatternRotate] SpatternRotate rotate: SS current iteration: %d/%d", superstate.iteration_count, SS::total_iterations()); - - // float offset = 0; - // float angle = 0; - // if (superstate.direction() == TDirection::LEFT) - // angle = -90 - offset; - // else - // angle = 90 + offset; - - // this->configure(angle); - // this->configure(); + } }; } // namespace s_pattern_states From d8cb6e923ab5b8bc8cca38d5329a0deb49c69f76 Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Fri, 18 Aug 2023 09:44:37 -0700 Subject: [PATCH 69/70] refactoring files --- .../docker/{ => older}/run_docker_container_bash.sh | 0 .../docker/{ => older}/run_docker_container_editor.sh | 0 .../docker/{ => older}/run_editor_smacc.sh | 0 .../docker/{ => older}/run_docker_container_bash.sh | 0 .../docker/{ => older}/run_docker_container_editor.sh | 0 .../sm_dancebot_ue/docker/{ => older}/run_editor_smacc.sh | 0 .../docker/run_docker_container_bash_development.sh | 2 +- 7 files changed, 1 insertion(+), 1 deletion(-) rename smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/{ => older}/run_docker_container_bash.sh (100%) rename smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/{ => older}/run_docker_container_editor.sh (100%) rename smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/{ => older}/run_editor_smacc.sh (100%) rename smacc2_sm_reference_library/sm_dancebot_ue/docker/{ => older}/run_docker_container_bash.sh (100%) rename smacc2_sm_reference_library/sm_dancebot_ue/docker/{ => older}/run_docker_container_editor.sh (100%) rename smacc2_sm_reference_library/sm_dancebot_ue/docker/{ => older}/run_editor_smacc.sh (100%) diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/run_docker_container_bash.sh b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/older/run_docker_container_bash.sh similarity index 100% rename from smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/run_docker_container_bash.sh rename to smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/older/run_docker_container_bash.sh diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/run_docker_container_editor.sh b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/older/run_docker_container_editor.sh similarity index 100% rename from smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/run_docker_container_editor.sh rename to smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/older/run_docker_container_editor.sh diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/run_editor_smacc.sh b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/older/run_editor_smacc.sh similarity index 100% rename from smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/run_editor_smacc.sh rename to smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/older/run_editor_smacc.sh diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_bash.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/older/run_docker_container_bash.sh similarity index 100% rename from smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_bash.sh rename to smacc2_sm_reference_library/sm_dancebot_ue/docker/older/run_docker_container_bash.sh diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_editor.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/older/run_docker_container_editor.sh similarity index 100% rename from smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_editor.sh rename to smacc2_sm_reference_library/sm_dancebot_ue/docker/older/run_docker_container_editor.sh diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_editor_smacc.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/older/run_editor_smacc.sh similarity index 100% rename from smacc2_sm_reference_library/sm_dancebot_ue/docker/run_editor_smacc.sh rename to smacc2_sm_reference_library/sm_dancebot_ue/docker/older/run_editor_smacc.sh diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_bash_development.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_bash_development.sh index f33ee1e08..fb6ca4edb 100755 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_bash_development.sh +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_bash_development.sh @@ -5,4 +5,4 @@ DIR="$(dirname "$(realpath "$0")")" ROOT_DIR=`realpath $DIR/../../../..` DRIVER=$(apt list --installed | grep nvidia-driver) -sudo nvidia-docker run --net host --ipc=host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name ue_editor_rclue -v $ROOT_DIR/ue_project_1:/home/ros2_ws/src/ue_project_1 -v $ROOT_DIR/SMACC2:/home/ros2_ws/src/SMACC2 -v $ROOT_DIR/UE-Plugins:/home/ros2_ws/src/ue_project_1/Plugins/RapyutaSimulationPlugins -v $ROOT_DIR/rclUE:/home/ros2_ws/src/ue_project_1/Plugins/rclUE -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash -c "./nvidia-driver-check.sh '$DRIVER' && bash" +sudo nvidia-docker run --net host --ipc=host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name ue_editor_rclue -v $ROOT_DIR/ue_project_1:/home/ros2_ws/src/ue_project_1 -v $ROOT_DIR/ue_project_2:/home/ros2_ws/src/ue_project_2 -v $ROOT_DIR/SMACC2:/home/ros2_ws/src/SMACC2 -v $ROOT_DIR/UE-Plugins:/home/ros2_ws/src/ue_project_1/Plugins/RapyutaSimulationPlugins -v $ROOT_DIR/rclUE:/home/ros2_ws/src/ue_project_1/Plugins/rclUE -v $ROOT_DIR/UE-Plugins:/home/ros2_ws/src/ue_project_2/Plugins/RapyutaSimulationPlugins -v $ROOT_DIR/rclUE:/home/ros2_ws/src/ue_project_2/Plugins/rclUE -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash -c "./nvidia-driver-check.sh '$DRIVER' && bash" \ No newline at end of file From 329dbdc3ea049fd30543978eab42c905c47bd89a Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Sat, 19 Aug 2023 00:59:12 +0200 Subject: [PATCH 70/70] updating docker --- .../sm_dancebot_ue/docker/Dockerfile | 14 +++++++----- .../sm_dancebot_ue/docker/build_docker.sh | 22 +++++++++++++++++++ .../run_docker_container_bash_development.sh | 2 +- .../docker/update_project_files.sh | 2 +- 4 files changed, 32 insertions(+), 8 deletions(-) diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile b/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile index 3302e7fa0..1ed11f138 100644 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile @@ -53,7 +53,7 @@ RUN echo "copying current code version to docker image:" COPY --chown=$USER:$USER smacc2_sm_reference_library/sm_dancebot_ue/docker/UE5.1/UnrealEngine /home/$USER/src/UE5.1/UnrealEngine ENV UE5_DIR=/home/$USER/src/UE5.1/UnrealEngine -ENV PROJECT_BRANCH=smacc2_devel_stable +ENV PROJECT_BRANCH=main ENV PROJECT_NAME=ue_project_1 RUN sudo apt-get install -y wget gpg @@ -77,7 +77,7 @@ WORKDIR "/home/$USER/src/${PROJECT_NAME}" RUN git-lfs pull && git submodule foreach git-lfs pull RUN ls && echo ${UE5_DIR} -RUN ./update_project_files.sh +COPY --chown=$USER:$USER smacc2_sm_reference_library/sm_dancebot_ue/docker/update_project_files.sh update_project_files.sh ENV UE5_DIR=/home/$USER/src/UE5.1 # installing Dependencies @@ -90,11 +90,13 @@ ENV LD_LIBRARY_PATH=/usr/local/cuda/lib64:/usr/lib/x86_64-linux-gnu/gazebo-11/pl ENV UE5_DIR=/home/$USER/src/UE5.1/UnrealEngine COPY --chown=$USER:$USER smacc2_sm_reference_library/sm_dancebot_ue/docker/nvidia-driver-check.sh /home/$USER/src/${PROJECT_NAME}/nvidia-driver-check.sh -RUN cat Makefile -RUN ./update_project_files.sh -#RUN bash +WORKDIR /home/$USER/src/${PROJECT_NAME} +RUN echo "nvidia-driver-check.sh" +# RUN cat update_project_files.sh +# RUN ../UE5.1/UnrealEngine/Engine/Build/BatchFiles/Linux/GenerateProjectFiles.sh -CurrentPlatform ue_project_1.uproject +# #RUN bash -RUN make ${PROJECT_NAME}Editor +# RUN make ${PROJECT_NAME}Editor # # COPY smacc2 /home/$USER/src/SMACC2/smacc2 # RUN mkdir -p /home/$USER/src/SMACC2/ diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/build_docker.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/build_docker.sh index da6efd900..da9ede5fb 100755 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/build_docker.sh +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/build_docker.sh @@ -17,5 +17,27 @@ echo "git branch: $GIT_BRANCH" echo "ubuntu version: $UBUNTU_VERSION" echo "root path: $ROOT_DIR" +UE_FOLDER=$DIR/UE5.1/UnrealEngine +# if DIRECTORY does exist show message error +if [ -d "$UE_FOLDER" ]; then + echo "UE5.1 folder exists" +else + echo "UE5.1 folder does not exist" + echo "Please download UE5.1 from https://www.unrealengine.com/en-US/download" + echo "and extract it to $UE_FOLDER" + exit 1 +fi + +#it is expected to exist $UE_FOLDER/Engine +if [ -d "$UE_FOLDER/Engine" ]; then + echo "UE5.1 Engine folder exists" +else + echo "UE5.1 Engine folder does not exist" + echo "Please download UE5.1 from https://www.unrealengine.com/en-US/download" + echo "and extract it to $UE_FOLDER" + exit 1 +fi + + cd $ROOT_DIR sudo docker build --build-arg ROS_DISTRO=$ROS_DISTRO --build-arg GIT_BRANCH=$GIT_BRANCH --build-arg UBUNTU_VERSION=$UBUNTU_VERSION -t ue_editor_rclue:$ROS_DISTRO -f $ROOT_DIR/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile . $NOCACHE diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_bash_development.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_bash_development.sh index fb6ca4edb..257b64e62 100755 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_bash_development.sh +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_bash_development.sh @@ -5,4 +5,4 @@ DIR="$(dirname "$(realpath "$0")")" ROOT_DIR=`realpath $DIR/../../../..` DRIVER=$(apt list --installed | grep nvidia-driver) -sudo nvidia-docker run --net host --ipc=host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name ue_editor_rclue -v $ROOT_DIR/ue_project_1:/home/ros2_ws/src/ue_project_1 -v $ROOT_DIR/ue_project_2:/home/ros2_ws/src/ue_project_2 -v $ROOT_DIR/SMACC2:/home/ros2_ws/src/SMACC2 -v $ROOT_DIR/UE-Plugins:/home/ros2_ws/src/ue_project_1/Plugins/RapyutaSimulationPlugins -v $ROOT_DIR/rclUE:/home/ros2_ws/src/ue_project_1/Plugins/rclUE -v $ROOT_DIR/UE-Plugins:/home/ros2_ws/src/ue_project_2/Plugins/RapyutaSimulationPlugins -v $ROOT_DIR/rclUE:/home/ros2_ws/src/ue_project_2/Plugins/rclUE -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash -c "./nvidia-driver-check.sh '$DRIVER' && bash" \ No newline at end of file +sudo nvidia-docker run --net host --ipc=host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name ue_editor_rclue -v $ROOT_DIR/ue_project_1:/home/ros2_ws/src/ue_project_1 -v $ROOT_DIR/ue_project_2:/home/ros2_ws/src/ue_project_2 -v $ROOT_DIR/SMACC2:/home/ros2_ws/src/SMACC2 -v $ROOT_DIR/UE-Plugins:/home/ros2_ws/src/ue_project_1/Plugins/RapyutaSimulationPlugins -v $ROOT_DIR/rclUE:/home/ros2_ws/src/ue_project_1/Plugins/rclUE -v $ROOT_DIR/UE-Plugins:/home/ros2_ws/src/ue_project_2/Plugins/RapyutaSimulationPlugins -v $ROOT_DIR/rclUE:/home/ros2_ws/src/ue_project_2/Plugins/rclUE -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash -c "./nvidia-driver-check.sh '$DRIVER' && bash" diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/update_project_files.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/update_project_files.sh index 086a474a3..0abd5e552 100755 --- a/smacc2_sm_reference_library/sm_dancebot_ue/docker/update_project_files.sh +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/update_project_files.sh @@ -1,2 +1,2 @@ #!/bin/bash -/home/ros2_ws/src/UE5.1/Engine/Build/BatchFiles/Linux/GenerateProjectFiles.sh: +/home/ros2_ws/src/UE5.1/Engine/Build/BatchFiles/Linux/GenerateProjectFiles.sh