nitros client creation (#560)

* nitros client creation

* precommit

* sm_panda_inventory keyboard addition

* Adding keyboard server to arm launch files

* sm_panda_1 tune up

---------

Co-authored-by: brettpac <brettpac@system76-pc.localdomain>

smacc2_client_library/nitrosz_client/nitrosz_client/CMakeLists.txt

@@ -0,0 +1,119 @@
+cmake_minimum_required(VERSION 3.5)
+project(nitrosz_client)
+
+# 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(smacc2 REQUIRED)
+find_package(std_srvs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(nav2_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(angles REQUIRED)
+find_package(yaml_cpp_vendor REQUIRED)
+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)
+find_package(rclcpp_action REQUIRED)
+find_package(nav2_util)
+
+
+set(dependencies
+  smacc2
+  std_srvs
+  std_msgs
+  nav2_msgs
+  tf2
+  tf2_ros
+  angles
+  yaml_cpp_vendor
+  tf2_geometry_msgs
+  bond
+  slam_toolbox
+  ament_index_cpp
+  rclcpp_action
+  nav2_util
+)
+
+include_directories(include)
+
+add_library(${PROJECT_NAME} SHARED
+  src/${PROJECT_NAME}/nitrosz_client.cpp
+  src/${PROJECT_NAME}/common.cpp
+
+  src/${PROJECT_NAME}/client_behaviors/cb_nitrosz_client_behavior_base.cpp
+  src/${PROJECT_NAME}/client_behaviors/cb_absolute_rotate.cpp
+  src/${PROJECT_NAME}/client_behaviors/cb_rotate_look_at.cpp
+  src/${PROJECT_NAME}/client_behaviors/cb_navigate_backward.cpp
+  src/${PROJECT_NAME}/client_behaviors/cb_navigate_forward.cpp
+  src/${PROJECT_NAME}/client_behaviors/cb_abort_navigation.cpp
+  src/${PROJECT_NAME}/client_behaviors/cb_navigate_global_position.cpp
+  src/${PROJECT_NAME}/client_behaviors/cb_navigate_next_waypoint.cpp
+  src/${PROJECT_NAME}/client_behaviors/cb_navigate_next_waypoint_until_reached.cpp
+  src/${PROJECT_NAME}/client_behaviors/cb_navigate_named_waypoint.cpp
+  src/${PROJECT_NAME}/client_behaviors/cb_rotate.cpp
+  src/${PROJECT_NAME}/client_behaviors/cb_undo_path_backwards.cpp
+  src/${PROJECT_NAME}/client_behaviors/cb_wait_pose.cpp
+  src/${PROJECT_NAME}/client_behaviors/cb_wait_nitros_nodes.cpp
+  src/${PROJECT_NAME}/client_behaviors/cb_pause_slam.cpp
+  src/${PROJECT_NAME}/client_behaviors/cb_resume_slam.cpp
+  src/${PROJECT_NAME}/client_behaviors/cb_wait_transform.cpp
+  src/${PROJECT_NAME}/client_behaviors/cb_seek_waypoint.cpp
+
+  # COMPONENTS
+  src/${PROJECT_NAME}/components/costmap_switch/cp_costmap_switch.cpp
+  #src/${PROJECT_NAME}/components/odom_tracker/cp_odom_tracker_node.cpp
+
+  src/${PROJECT_NAME}/components/odom_tracker/cp_odom_tracker.cpp
+  src/${PROJECT_NAME}/components/planner_switcher/cp_planner_switcher.cpp
+  src/${PROJECT_NAME}/components/goal_checker_switcher/cp_goal_checker_switcher.cpp
+  src/${PROJECT_NAME}/components/pose/cp_pose.cpp
+  src/${PROJECT_NAME}/components/waypoints_navigator/cp_waypoints_event_dispatcher.cpp
+  src/${PROJECT_NAME}/components/waypoints_navigator/cp_waypoints_navigator.cpp
+  src/${PROJECT_NAME}/components/waypoints_navigator/cp_waypoints_visualizer.cpp
+  src/${PROJECT_NAME}/components/amcl/cp_amcl.cpp
+  src/${PROJECT_NAME}/components/slam_toolbox/cp_slam_toolbox.cpp
+
+  src/${PROJECT_NAME}/client_behaviors/cb_active_stop.cpp
+  src/${PROJECT_NAME}/client_behaviors/cb_load_waypoints_file.cpp
+  src/${PROJECT_NAME}/client_behaviors/cb_navigate_next_waypoint_free.cpp
+  src/${PROJECT_NAME}/client_behaviors/cb_position_control_free_space.cpp
+  src/${PROJECT_NAME}/client_behaviors/cb_pure_spinning.cpp
+  src/${PROJECT_NAME}/client_behaviors/cb_save_slam_map.cpp
+)
+
+ament_target_dependencies(${PROJECT_NAME}
+                          ${dependencies})
+
+target_link_libraries(${PROJECT_NAME})
+
+ament_export_include_directories(include)
+ament_export_libraries(${PROJECT_NAME})
+ament_export_dependencies(${dependencies})
+
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(DIRECTORY include/
+  DESTINATION include/
+)
+
+install(FILES scripts/lidar_completion.py
+  DESTINATION lib/${PROJECT_NAME}/
+)
+
+ament_package()

smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors.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.
+
+#pragma once
+
+// rotation behavior
+#include <nitrosz_client/client_behaviors/cb_absolute_rotate.hpp>
+#include <nitrosz_client/client_behaviors/cb_rotate.hpp>
+#include <nitrosz_client/client_behaviors/cb_rotate_look_at.hpp>
+
+#include <nitrosz_client/client_behaviors/cb_navigate_backwards.hpp>
+#include <nitrosz_client/client_behaviors/cb_navigate_forward.hpp>
+#include <nitrosz_client/client_behaviors/cb_navigate_global_position.hpp>
+#include <nitrosz_client/client_behaviors/cb_retry_behavior.hpp>
+#include <nitrosz_client/client_behaviors/cb_undo_path_backwards.hpp>
+
+// nav2 synchronization behaviors
+#include <nitrosz_client/client_behaviors/cb_wait_nav2_nodes.hpp>
+#include <nitrosz_client/client_behaviors/cb_wait_pose.hpp>
+#include <nitrosz_client/client_behaviors/cb_wait_transform.hpp>
+
+// others
+#include <nitrosz_client/client_behaviors/cb_abort_navigation.hpp>
+#include <nitrosz_client/client_behaviors/cb_stop_navigation.hpp>
+
+// waypoints behaviors
+#include <nitrosz_client/client_behaviors/cb_navigate_named_waypoint.hpp>
+#include <nitrosz_client/client_behaviors/cb_navigate_next_waypoint.hpp>
+#include <nitrosz_client/client_behaviors/cb_navigate_next_waypoint_until_reached.hpp>
+#include <nitrosz_client/client_behaviors/cb_seek_waypoint.hpp>
+
+// slam behaviors
+#include <nitrosz_client/client_behaviors/cb_pause_slam.hpp>
+#include <nitrosz_client/client_behaviors/cb_resume_slam.hpp>

smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_abort_navigation.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 <tf2_ros/buffer.h>
+
+#include "cb_navigate_global_position.hpp"
+#include "cb_nitrosz_client_behavior_base.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+
+namespace cl_nitrosz
+{
+class CbAbortNavigation : public smacc2::SmaccAsyncClientBehavior
+{
+public:
+  CbAbortNavigation();
+
+  template <typename TOrthogonal, typename TSourceObject>
+  void onOrthogonalAllocation()
+  {
+    this->requiresClient(nitroszClient_);
+    smacc2::SmaccAsyncClientBehavior::onOrthogonalAllocation<TOrthogonal, TSourceObject>();
+  }
+
+  void onEntry() override;
+  void onExit() override;
+
+private:
+  cl_nitrosz::ClNitrosZ * nitroszClient_;
+};
+}  // namespace cl_nitrosz

smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_absolute_rotate.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 <tf2_ros/buffer.h>
+
+#include "cb_nitrosz_client_behavior_base.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+
+namespace cl_nitrosz
+{
+class CbAbsoluteRotate : public CbNav2ZClientBehaviorBase
+{
+public:
+  std::shared_ptr<tf2_ros::Buffer> listener;
+
+  float absoluteGoalAngleDegree;
+  std::optional<float> yawGoalTolerance;
+  std::optional<float> maxVelTheta;  // if not defined, default parameter server
+  std::optional<SpinningPlanner> spinningPlanner;
+
+  CbAbsoluteRotate();
+  CbAbsoluteRotate(float absoluteGoalAngleDegree, float yawGoalTolerance = -1);
+
+  void onEntry() override;
+  void onExit() override;
+
+private:
+  void updateTemporalBehaviorParameters(bool undo);
+  float oldYawTolerance;
+  float oldMaxVelTheta;
+  float oldMinVelTheta;
+};
+}  // namespace cl_nitrosz

smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_active_stop.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 <geometry_msgs/msg/twist.hpp>
+#include <smacc2/smacc_asynchronous_client_behavior.hpp>
+
+namespace cl_nitrosz
+{
+struct CbActiveStop : public smacc2::SmaccAsyncClientBehavior
+{
+private:
+  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
+
+public:
+  CbActiveStop();
+
+  void onEntry() override;
+
+  void onExit() override;
+};
+}  // namespace cl_nitrosz

smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_load_waypoints_file.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 <nitrosz_client/components/waypoints_navigator/cp_waypoints_navigator_base.hpp>
+#include <smacc2/smacc_client_behavior.hpp>
+
+namespace cl_nitrosz
+{
+struct CbLoadWaypointsFile : public smacc2::SmaccAsyncClientBehavior
+{
+public:
+  CbLoadWaypointsFile(std::string filepath);
+
+  CbLoadWaypointsFile(std::string parameter_name, std::string packagenamespace);
+
+  void onEntry() override;
+
+  void onExit() override;
+
+  std::optional<std::string> filepath_;
+
+  std::optional<std::string> parameterName_;
+  std::optional<std::string> packageNamespace_;
+
+  cl_nitrosz::CpWaypointNavigatorBase * waypointsNavigator_;
+};
+}  // namespace cl_nitrosz