HiveCoreRD/hivecore_robot_brain
6
0
Fork 0
Code Issues Pull Requests 1 Actions Packages Projects Releases Wiki Activity

delete no used smacc2 code

Browse Source
This commit is contained in:
david
2025-10-28 09:07:45 +08:00
parent 67e802cc02
commit b2a7bada15
1118 changed files with 0 additions and 660429 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1035
src/thirdparty/SMACC2-humble/smacc2_client_library/CLAUDE.md vendored
View File

File diff suppressed because it is too large Load Diff

2546
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_keyboard/CHANGELOG.rst vendored
View File

File diff suppressed because it is too large Load Diff

48
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_keyboard/CMakeLists.txt vendored
View File

@@ -1,48 +0,0 @@
cmake_minimum_required(VERSION 3.5)
project(cl_keyboard)
# 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_msgs REQUIRED)
include_directories(
include
${smacc2_INCLUDE_DIRS}
${std_msgs_INCLUDE_DIRS}
)
file(GLOB_RECURSE SRC_FILES src *.cpp)
#file(GLOB_RECURSE SRC_FILES "src/*.cpp" "src/client_behaviors/*.cpp")
add_library(${PROJECT_NAME}
${SRC_FILES}
)
target_link_libraries(${PROJECT_NAME} ${smacc2_LIBRARIES} ${std_msgs_LIBRARIES})
ament_target_dependencies(${PROJECT_NAME} smacc2 std_msgs)
ament_export_include_directories(include)
ament_export_libraries(${PROJECT_NAME})
install(
DIRECTORY include/
DESTINATION include)
install(TARGETS
${PROJECT_NAME}
DESTINATION lib/)
install(PROGRAMS
servers/keyboard_server_node.py
DESTINATION lib/${PROJECT_NAME})
ament_package()

58
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_keyboard/include/cl_keyboard/cl_keyboard.hpp vendored
View File

@@ -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.
#pragma once
#include <cl_keyboard/components/cp_keyboard_listener_1.hpp>
#include <smacc2/client_core_components/cp_topic_subscriber.hpp>
#include <smacc2/introspection/introspection.hpp>
#include <smacc2/smacc.hpp>
#include <boost/asio.hpp>
#include <boost/asio/posix/stream_descriptor.hpp>
#include <iostream>
#include <std_msgs/msg/u_int16.hpp>
namespace cl_keyboard
{
class ClKeyboard : public smacc2::ISmaccClient
{
public:
ClKeyboard();
virtual ~ClKeyboard();
// Override the base class methods to call our setup
template <typename TOrthogonal, typename TClient>
void onComponentInitialization()
// Clients utilize a composition based architecture for their components.
// In the function body below we create the components that will be used in this client.
{
// We start by creating a topic subscriber component from SMACC2s client core components.
// We use this to gain the topic funcionality interated with SMACC and the ability to post smacc events for transitions.
// We are using it to handle the reception of ros topic messages and to notify the other components in the client.
this->createComponent<
smacc2::client_core_components::CpTopicSubscriber<std_msgs::msg::UInt16>, TOrthogonal,
ClKeyboard>("/keyboard_unicode");
// This keyboard listener component requires CpTopicSubscriber.
// It is notified by the CpTopicSubscriber and processes the messages to decide which keyboard event must be posted and then posts it.
this->createComponent<cl_keyboard::components::CpKeyboardListener1, TOrthogonal, ClKeyboard>();
}
};
} // namespace cl_keyboard

110
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_keyboard/include/cl_keyboard/client_behaviors/cb_default_keyboard_behavior.hpp vendored
View File

@@ -1,110 +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.
#pragma once
#include <cl_keyboard/cl_keyboard.hpp>
#include <smacc2/smacc_client_behavior.hpp>
#include <std_msgs/msg/u_int16.hpp>
namespace cl_keyboard
{
using namespace cl_keyboard::components;
class CbDefaultKeyboardBehavior : public smacc2::SmaccClientBehavior
{
public:
components::CpKeyboardListener1 * cpSubscriber1;
std::function<void(char)> postEventKeyPress;
void onEntry()
{
this->requiresComponent(this->cpSubscriber1);
this->cpSubscriber1->OnKeyPress(&CbDefaultKeyboardBehavior::OnKeyPress, this);
}
template <typename TOrthogonal, typename TSourceObject>
void onStateOrthogonalAllocation()
{
postEventKeyPress = [=](char character)
{
if (character == 'a')
postKeyEvent<EvKeyPressA<CbDefaultKeyboardBehavior, TOrthogonal>>();
else if (character == 'b')
postKeyEvent<EvKeyPressB<CbDefaultKeyboardBehavior, TOrthogonal>>();
else if (character == 'c')
postKeyEvent<EvKeyPressC<CbDefaultKeyboardBehavior, TOrthogonal>>();
else if (character == 'd')
postKeyEvent<EvKeyPressD<CbDefaultKeyboardBehavior, TOrthogonal>>();
else if (character == 'e')
postKeyEvent<EvKeyPressE<CbDefaultKeyboardBehavior, TOrthogonal>>();
else if (character == 'f')
postKeyEvent<EvKeyPressF<CbDefaultKeyboardBehavior, TOrthogonal>>();
else if (character == 'g')
postKeyEvent<EvKeyPressG<CbDefaultKeyboardBehavior, TOrthogonal>>();
else if (character == 'h')
postKeyEvent<EvKeyPressH<CbDefaultKeyboardBehavior, TOrthogonal>>();
else if (character == 'i')
postKeyEvent<EvKeyPressI<CbDefaultKeyboardBehavior, TOrthogonal>>();
else if (character == 'j')
postKeyEvent<EvKeyPressJ<CbDefaultKeyboardBehavior, TOrthogonal>>();
else if (character == 'k')
postKeyEvent<EvKeyPressK<CbDefaultKeyboardBehavior, TOrthogonal>>();
else if (character == 'l')
postKeyEvent<EvKeyPressL<CbDefaultKeyboardBehavior, TOrthogonal>>();
else if (character == 'm')
postKeyEvent<EvKeyPressM<CbDefaultKeyboardBehavior, TOrthogonal>>();
else if (character == 'n')
postKeyEvent<EvKeyPressN<CbDefaultKeyboardBehavior, TOrthogonal>>();
else if (character == 'o')
postKeyEvent<EvKeyPressO<CbDefaultKeyboardBehavior, TOrthogonal>>();
else if (character == 'p')
postKeyEvent<EvKeyPressP<CbDefaultKeyboardBehavior, TOrthogonal>>();
else if (character == 'q')
postKeyEvent<EvKeyPressQ<CbDefaultKeyboardBehavior, TOrthogonal>>();
else if (character == 'r')
postKeyEvent<EvKeyPressR<CbDefaultKeyboardBehavior, TOrthogonal>>();
else if (character == 's')
postKeyEvent<EvKeyPressS<CbDefaultKeyboardBehavior, TOrthogonal>>();
else if (character == 't')
postKeyEvent<EvKeyPressT<CbDefaultKeyboardBehavior, TOrthogonal>>();
else if (character == 'u')
postKeyEvent<EvKeyPressU<CbDefaultKeyboardBehavior, TOrthogonal>>();
else if (character == 'v')
postKeyEvent<EvKeyPressV<CbDefaultKeyboardBehavior, TOrthogonal>>();
else if (character == 'w')
postKeyEvent<EvKeyPressW<CbDefaultKeyboardBehavior, TOrthogonal>>();
else if (character == 'x')
postKeyEvent<EvKeyPressX<CbDefaultKeyboardBehavior, TOrthogonal>>();
else if (character == 'y')
postKeyEvent<EvKeyPressY<CbDefaultKeyboardBehavior, TOrthogonal>>();
else if (character == 'z')
postKeyEvent<EvKeyPressZ<CbDefaultKeyboardBehavior, TOrthogonal>>();
};
}
void OnKeyPress(char character) { postEventKeyPress(character); }
template <typename TEv>
void postKeyEvent()
{
RCLCPP_WARN(
getLogger(), "CbDefaultKeyboardBehavior %ld ev: %s", (long)(void *)this,
smacc2::demangleSymbol(typeid(TEv).name()).c_str());
auto event = new TEv();
this->postEvent(event);
}
};
} // namespace cl_keyboard

269
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_keyboard/include/cl_keyboard/components/cp_keyboard_listener_1.hpp vendored
View File

@@ -1,269 +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.
#pragma once
#include <smacc2/client_core_components/cp_topic_subscriber.hpp>
#include <smacc2/introspection/introspection.hpp>
#include <smacc2/smacc.hpp>
#include <boost/asio.hpp>
#include <boost/asio/posix/stream_descriptor.hpp>
#include <iostream>
#include <std_msgs/msg/u_int16.hpp>
namespace cl_keyboard::components
{
//----------------- KEYBOARD EVENT DEFINITIONS ----------------------------------------------
template <typename TSource, typename TOrthogonal>
struct EvKeyPressA : sc::event<EvKeyPressA<TSource, TOrthogonal>>
{
};
template <typename TSource, typename TOrthogonal>
struct EvKeyPressB : sc::event<EvKeyPressB<TSource, TOrthogonal>>
{
};
template <typename TSource, typename TOrthogonal>
struct EvKeyPressC : sc::event<EvKeyPressC<TSource, TOrthogonal>>
{
};
template <typename TSource, typename TOrthogonal>
struct EvKeyPressD : sc::event<EvKeyPressD<TSource, TOrthogonal>>
{
};
template <typename TSource, typename TOrthogonal>
struct EvKeyPressE : sc::event<EvKeyPressE<TSource, TOrthogonal>>
{
};
template <typename TSource, typename TOrthogonal>
struct EvKeyPressF : sc::event<EvKeyPressF<TSource, TOrthogonal>>
{
};
template <typename TSource, typename TOrthogonal>
struct EvKeyPressG : sc::event<EvKeyPressG<TSource, TOrthogonal>>
{
};
template <typename TSource, typename TOrthogonal>
struct EvKeyPressH : sc::event<EvKeyPressH<TSource, TOrthogonal>>
{
};
template <typename TSource, typename TOrthogonal>
struct EvKeyPressI : sc::event<EvKeyPressI<TSource, TOrthogonal>>
{
};
template <typename TSource, typename TOrthogonal>
struct EvKeyPressJ : sc::event<EvKeyPressJ<TSource, TOrthogonal>>
{
};
template <typename TSource, typename TOrthogonal>
struct EvKeyPressK : sc::event<EvKeyPressK<TSource, TOrthogonal>>
{
};
template <typename TSource, typename TOrthogonal>
struct EvKeyPressL : sc::event<EvKeyPressL<TSource, TOrthogonal>>
{
};
template <typename TSource, typename TOrthogonal>
struct EvKeyPressM : sc::event<EvKeyPressM<TSource, TOrthogonal>>
{
};
template <typename TSource, typename TOrthogonal>
struct EvKeyPressN : sc::event<EvKeyPressN<TSource, TOrthogonal>>
{
};
template <typename TSource, typename TOrthogonal>
struct EvKeyPressO : sc::event<EvKeyPressO<TSource, TOrthogonal>>
{
};
template <typename TSource, typename TOrthogonal>
struct EvKeyPressP : sc::event<EvKeyPressP<TSource, TOrthogonal>>
{
};
template <typename TSource, typename TOrthogonal>
struct EvKeyPressQ : sc::event<EvKeyPressQ<TSource, TOrthogonal>>
{
};
template <typename TSource, typename TOrthogonal>
struct EvKeyPressR : sc::event<EvKeyPressR<TSource, TOrthogonal>>
{
};
template <typename TSource, typename TOrthogonal>
struct EvKeyPressS : sc::event<EvKeyPressS<TSource, TOrthogonal>>
{
};
template <typename TSource, typename TOrthogonal>
struct EvKeyPressT : sc::event<EvKeyPressT<TSource, TOrthogonal>>
{
};
template <typename TSource, typename TOrthogonal>
struct EvKeyPressU : sc::event<EvKeyPressU<TSource, TOrthogonal>>
{
};
template <typename TSource, typename TOrthogonal>
struct EvKeyPressV : sc::event<EvKeyPressV<TSource, TOrthogonal>>
{
};
template <typename TSource, typename TOrthogonal>
struct EvKeyPressW : sc::event<EvKeyPressW<TSource, TOrthogonal>>
{
};
template <typename TSource, typename TOrthogonal>
struct EvKeyPressX : sc::event<EvKeyPressX<TSource, TOrthogonal>>
{
};
template <typename TSource, typename TOrthogonal>
struct EvKeyPressY : sc::event<EvKeyPressY<TSource, TOrthogonal>>
{
};
template <typename TSource, typename TOrthogonal>
struct EvKeyPressZ : sc::event<EvKeyPressZ<TSource, TOrthogonal>>
{
};
//------------------ KEYBOARD LISTENER COMPONENT ---------------------------------------------
class CpKeyboardListener1 : public smacc2::ISmaccComponent
{
public:
CpKeyboardListener1() {}
virtual ~CpKeyboardListener1() {}
// Override the base class methods to call our setup
template <typename TOrthogonal, typename TClient>
void onComponentInitialization()
{
RCLCPP_INFO(getLogger(), "CpKeyboardListener1 initialization");
smacc2::client_core_components::CpTopicSubscriber<std_msgs::msg::UInt16> * subscriber;
this->requiresComponent(subscriber);
subscriber->onMessageReceived(&CpKeyboardListener1::onKeyboardMessage, this);
postEventKeyPress = [=](auto unicode_keychar)
{
char character = (char)unicode_keychar.data;
RCLCPP_WARN(getLogger(), "detected keyboard: %c", character);
if (character == 'a')
this->postKeyEvent<EvKeyPressA<CpKeyboardListener1, TOrthogonal>>();
else if (character == 'b')
this->postKeyEvent<EvKeyPressB<CpKeyboardListener1, TOrthogonal>>();
else if (character == 'c')
this->postKeyEvent<EvKeyPressC<CpKeyboardListener1, TOrthogonal>>();
else if (character == 'd')
this->postKeyEvent<EvKeyPressD<CpKeyboardListener1, TOrthogonal>>();
else if (character == 'e')
this->postKeyEvent<EvKeyPressE<CpKeyboardListener1, TOrthogonal>>();
else if (character == 'f')
this->postKeyEvent<EvKeyPressF<CpKeyboardListener1, TOrthogonal>>();
else if (character == 'g')
this->postKeyEvent<EvKeyPressG<CpKeyboardListener1, TOrthogonal>>();
else if (character == 'h')
this->postKeyEvent<EvKeyPressH<CpKeyboardListener1, TOrthogonal>>();
else if (character == 'i')
this->postKeyEvent<EvKeyPressI<CpKeyboardListener1, TOrthogonal>>();
else if (character == 'j')
this->postKeyEvent<EvKeyPressJ<CpKeyboardListener1, TOrthogonal>>();
else if (character == 'k')
this->postKeyEvent<EvKeyPressK<CpKeyboardListener1, TOrthogonal>>();
else if (character == 'l')
this->postKeyEvent<EvKeyPressL<CpKeyboardListener1, TOrthogonal>>();
else if (character == 'm')
this->postKeyEvent<EvKeyPressM<CpKeyboardListener1, TOrthogonal>>();
else if (character == 'n')
this->postKeyEvent<EvKeyPressN<CpKeyboardListener1, TOrthogonal>>();
else if (character == 'o')
this->postKeyEvent<EvKeyPressO<CpKeyboardListener1, TOrthogonal>>();
else if (character == 'p')
this->postKeyEvent<EvKeyPressP<CpKeyboardListener1, TOrthogonal>>();
else if (character == 'q')
this->postKeyEvent<EvKeyPressQ<CpKeyboardListener1, TOrthogonal>>();
else if (character == 'r')
this->postKeyEvent<EvKeyPressR<CpKeyboardListener1, TOrthogonal>>();
else if (character == 's')
this->postKeyEvent<EvKeyPressS<CpKeyboardListener1, TOrthogonal>>();
else if (character == 't')
this->postKeyEvent<EvKeyPressT<CpKeyboardListener1, TOrthogonal>>();
else if (character == 'u')
this->postKeyEvent<EvKeyPressU<CpKeyboardListener1, TOrthogonal>>();
else if (character == 'v')
this->postKeyEvent<EvKeyPressV<CpKeyboardListener1, TOrthogonal>>();
else if (character == 'w')
this->postKeyEvent<EvKeyPressW<CpKeyboardListener1, TOrthogonal>>();
else if (character == 'x')
this->postKeyEvent<EvKeyPressX<CpKeyboardListener1, TOrthogonal>>();
else if (character == 'y')
this->postKeyEvent<EvKeyPressY<CpKeyboardListener1, TOrthogonal>>();
else if (character == 'z')
this->postKeyEvent<EvKeyPressZ<CpKeyboardListener1, TOrthogonal>>();
OnKeyPress_(character);
};
}
// This signal is created to be used in client behaviors
smacc2::SmaccSignal<void(char keypress)> OnKeyPress_;
// This is the signal subscription method for client behaviors and other components
template <typename T>
void OnKeyPress(void (T::*callback)(char keypress), T * object)
{
this->getStateMachine()->createSignalConnection(OnKeyPress_, callback, object);
}
void onKeyboardMessage(const std_msgs::msg::UInt16 & unicode_keychar)
{
postEventKeyPress(unicode_keychar);
}
public:
template <typename TEv>
void postKeyEvent()
{
RCLCPP_WARN(
getLogger(), "CpKeyboardListener1 ev: %s",
smacc2::demangleSymbol(typeid(TEv).name()).c_str());
this->postEvent<TEv>();
}
private:
std::function<void(std_msgs::msg::UInt16)> postEventKeyPress;
};
} // namespace cl_keyboard::components

18
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_keyboard/package.xml vendored
View File

@@ -1,18 +0,0 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>cl_keyboard</name>
<version>2.3.19</version>
<description>The cl_keyboard package</description>
<maintainer email="pablo@ibrobotics.com">Pablo Inigo Blasco</maintainer>
<license>Apache-2.0</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>smacc2</depend>
<depend>std_msgs</depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

85
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_keyboard/servers/keyboard_server_node.py vendored
View File

@@ -1,85 +0,0 @@
#!/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.
# *****************************************************************************************************************
# *
# * Authors: Pablo Inigo Blasco, Brett Aldrich
# *
# ******************************************************************************************************************/
import sys
import select
import termios
import tty
import rclpy
from rclpy.node import Node
from std_msgs.msg import UInt16
settings = termios.tcgetattr(sys.stdin)
def getKey():
global settings
tty.setraw(sys.stdin.fileno())
select.select([sys.stdin], [], [], 0)
key = sys.stdin.read(1)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
return key
class KeyboardPublisher(Node):
def __init__(self):
super().__init__("keyboard_node")
self.pub = self.create_publisher(UInt16, "keyboard_unicode", 1)
timer_period = 0.20 # seconds
self.timer = self.create_timer(timer_period, self.timer_update)
self.i = 0
def timer_update(self):
try:
key = getKey()
# rospy.loginfo(type(key))
msg = UInt16()
msg.data = ord(key)
self.get_logger().info(f"key: {msg.data}")
self.pub.publish(msg)
except Exception as e:
print(e)
finally:
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
def main(args=None):
rclpy.init(args=args)
node = KeyboardPublisher()
rclpy.spin(node)
# Destroy the node explicitly
# (optional - otherwise it will be done automatically
# when the garbage collector destroys the node object)
node.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
main()

29
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_keyboard/src/cl_keyboard/cl_keyboard.cpp vendored
View File

@@ -1,29 +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 <cl_keyboard/cl_keyboard.hpp>
namespace cl_keyboard
{
// Declare the Client's default constructor.
ClKeyboard::ClKeyboard() {}
// Declare the Client's default destructor.
ClKeyboard::~ClKeyboard() {}
} // namespace cl_keyboard

7724
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/CHANGELOG.rst vendored
View File

File diff suppressed because it is too large Load Diff

122
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/CMakeLists.txt vendored
View File

@@ -1,122 +0,0 @@
cmake_minimum_required(VERSION 3.5)
project(cl_nav2z)
# 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}/cl_nav2z.cpp
src/${PROJECT_NAME}/common.cpp
src/${PROJECT_NAME}/client_behaviors/cb_nav2z_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_nav2_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
src/${PROJECT_NAME}/client_behaviors/cb_spiral_motion.cpp
src/${PROJECT_NAME}/client_behaviors/cb_track_path_odometry.cpp
src/${PROJECT_NAME}/client_behaviors/cb_track_path_slam.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()

82
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/include/cl_nav2z/cl_nav2z.hpp vendored
View File

@@ -1,82 +0,0 @@
// Copyright 2024 Robosoft Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#pragma once
#include <cl_nav2z/components/nav2_action_interface/cp_nav2_action_interface.hpp>
#include <nav2_msgs/action/navigate_to_pose.hpp>
#include <smacc2/client_core_components/cp_action_client.hpp>
#include <smacc2/smacc.hpp>
namespace cl_nav2z
{
// Pure component-based ClNav2Z - No legacy support
// Follows cl_keyboard pattern: orchestrator only, behaviors use requiresComponent()
class ClNav2Z : public smacc2::ISmaccClient
{
public:
// Constructor
ClNav2Z(std::string actionServerName = "/navigate_to_pose") : actionServerName_(actionServerName)
{
}
virtual ~ClNav2Z() = default;
// Component composition during orthogonal initialization
template <typename TOrthogonal, typename TClient>
void onComponentInitialization()
{
// Create core action client component
auto actionClient = this->createComponent<
smacc2::client_core_components::CpActionClient<nav2_msgs::action::NavigateToPose>,
TOrthogonal, ClNav2Z>();
actionClient->actionServerName = actionServerName_;
// Create nav2-specific interface component (requires actionClient)
this->createComponent<components::CpNav2ActionInterface, TOrthogonal, ClNav2Z>();
RCLCPP_INFO_STREAM(
this->getLogger(), "[ClNav2Z] Components created for action server: " << actionServerName_);
}
private:
std::string actionServerName_;
// ✅ PURE ORCHESTRATOR PATTERN - ALL LEGACY METHODS REMOVED:
//
// ❌ REMOVED: sendGoal() methods - use CpNav2ActionInterface::sendGoal()
// ❌ REMOVED: signal connection proxy methods - use CpNav2ActionInterface::onNavigationSucceeded()
// ❌ REMOVED: signal getter methods - signals owned by components
// ❌ REMOVED: component accessor methods - use requiresComponent()
// ❌ REMOVED: convenience wrapper methods - use component APIs directly
// ❌ REMOVED: type aliases - use component types directly
//
// ✅ NEW BEHAVIOR PATTERN:
// class CbNavigateForward : public CbNav2ZClientBehaviorBase {
// void onEntry() override {
// CpNav2ActionInterface* navInterface;
// this->requiresComponent(navInterface);
// navInterface->onNavigationSucceeded(&CbNavigateForward::onSuccess, this);
// Goal goal = createForwardGoal();
// navInterface->sendGoal(goal);
// }
// };
//
// ✅ SMACC2 SIGNAL COMPLIANCE: All signal connections use createSignalConnection()
// ✅ FRAMEWORK CONSISTENCY: Matches cl_keyboard pure component pattern
// ✅ CLEAN SEPARATION: Client orchestrates, components implement, behaviors consume
};
} // namespace cl_nav2z

42
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/include/cl_nav2z/cl_nav2z.hpp.bak vendored
View File

@@ -1,42 +0,0 @@
// Copyright 2024 Robosoft Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#pragma once
#include <nav2_msgs/action/navigate_to_pose.hpp>
#include <smacc2/client_bases/smacc_action_client_base.hpp>
#include <smacc2/smacc.hpp>
namespace cl_nav2z
{
// Declare Client class, inherit from SmaccActionClientBase template with default action argument.
class ClNav2Z
: public smacc2::client_bases::SmaccActionClientBase<nav2_msgs::action::NavigateToPose>
{
public:
// Bring the SmaccActionClientBase types into the current scope.
using smacc2::client_bases::SmaccActionClientBase<nav2_msgs::action::NavigateToPose>::GoalHandle;
using smacc2::client_bases::SmaccActionClientBase<
nav2_msgs::action::NavigateToPose>::ResultCallback;
// Define "SmaccNavigateResultSignal" as a SmaccSignal that points to the WrappedResult type of SmaccActionClientBase.
typedef smacc2::SmaccSignal<void(const WrappedResult &)> SmaccNavigateResultSignal;
// Declare Client class constructor with string set to the right ROS action.
ClNav2Z(std::string navigateToPoseAction = "/navigate_to_pose");
// Declare Client class destructor.
virtual ~ClNav2Z();
};
} // namespace cl_nav2z

45
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/include/cl_nav2z/client_behaviors.hpp vendored
View File

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

49
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/include/cl_nav2z/client_behaviors/cb_abort_navigation.hpp vendored
View File

@@ -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 <tf2_ros/buffer.h>
#include <cl_nav2z/components/nav2_action_interface/cp_nav2_action_interface.hpp>
#include "cb_nav2z_client_behavior_base.hpp"
#include "cb_navigate_global_position.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
namespace cl_nav2z
{
class CbAbortNavigation : public smacc2::SmaccAsyncClientBehavior
{
public:
CbAbortNavigation();
template <typename TOrthogonal, typename TSourceObject>
void onStateOrthogonalAllocation()
{
this->requiresComponent(nav2ActionInterface_);
smacc2::SmaccAsyncClientBehavior::onStateOrthogonalAllocation<TOrthogonal, TSourceObject>();
}
void onEntry() override;
void onExit() override;
private:
components::CpNav2ActionInterface * nav2ActionInterface_ = nullptr;
};
} // namespace cl_nav2z

51
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/include/cl_nav2z/client_behaviors/cb_absolute_rotate.hpp vendored
View File

@@ -1,51 +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 <tf2_ros/buffer.h>
#include "cb_nav2z_client_behavior_base.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
namespace cl_nav2z
{
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_nav2z

40
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/include/cl_nav2z/client_behaviors/cb_active_stop.hpp vendored
View File

@@ -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 <geometry_msgs/msg/twist.hpp>
#include <smacc2/smacc_asynchronous_client_behavior.hpp>
namespace cl_nav2z
{
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_nav2z

46
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/include/cl_nav2z/client_behaviors/cb_load_waypoints_file.hpp vendored
View File

@@ -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
*
******************************************************************************************************************/
#pragma once
#include <cl_nav2z/components/waypoints_navigator/cp_waypoints_navigator_base.hpp>
#include <smacc2/smacc_client_behavior.hpp>
namespace cl_nav2z
{
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_nav2z::CpWaypointNavigatorBase * waypointsNavigator_;
};
} // namespace cl_nav2z

148
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/include/cl_nav2z/client_behaviors/cb_nav2z_client_behavior_base.hpp vendored
View File

@@ -1,148 +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 <cl_nav2z/cl_nav2z.hpp>
#include <cl_nav2z/components/nav2_action_interface/cp_nav2_action_interface.hpp>
#include <cl_nav2z/components/planner_switcher/cp_planner_switcher.hpp>
#include <smacc2/client_core_components/cp_action_client.hpp>
#include <smacc2/smacc_asynchronous_client_behavior.hpp>
namespace cl_nav2z
{
class CbNav2ZClientBehaviorBase : public smacc2::SmaccAsyncClientBehavior
{
public:
virtual ~CbNav2ZClientBehaviorBase();
template <typename TOrthogonal, typename TSourceObject>
void onStateOrthogonalAllocation()
{
// NEW: Pure component-based approach - no client dependencies
this->requiresComponent(nav2ActionInterface_);
this->requiresComponent(actionClient_);
smacc2::SmaccAsyncClientBehavior::onStateOrthogonalAllocation<TOrthogonal, TSourceObject>();
}
// LEGACY COMPATIBILITY: For third-party code that still calls the old method
template <typename TOrthogonal, typename TSourceObject>
[[deprecated(
"Use onStateOrthogonalAllocation instead. This method exists only for third-party "
"compatibility.")]] void
onOrthogonalAllocation()
{
onStateOrthogonalAllocation<TOrthogonal, TSourceObject>();
}
protected:
// NEW: Component-based API - uses components directly
void sendGoal(nav2_msgs::action::NavigateToPose::Goal & goal)
{
if (nav2ActionInterface_)
{
nav2ActionInterface_->sendGoal(goal);
}
}
void cancelGoal()
{
if (nav2ActionInterface_)
{
nav2ActionInterface_->cancelNavigation();
}
}
// Component-based signal connections
template <typename T>
boost::signals2::connection onNavigationSucceeded(
void (T::*callback)(const components::CpNav2ActionInterface::WrappedResult &), T * object)
{
if (nav2ActionInterface_)
{
return nav2ActionInterface_->onNavigationSucceeded(callback, object);
}
return boost::signals2::connection();
}
template <typename T>
boost::signals2::connection onNavigationAborted(
void (T::*callback)(const components::CpNav2ActionInterface::WrappedResult &), T * object)
{
if (nav2ActionInterface_)
{
return nav2ActionInterface_->onNavigationAborted(callback, object);
}
return boost::signals2::connection();
}
template <typename T>
boost::signals2::connection onNavigationCancelled(
void (T::*callback)(const components::CpNav2ActionInterface::WrappedResult &), T * object)
{
if (nav2ActionInterface_)
{
return nav2ActionInterface_->onNavigationCancelled(callback, object);
}
return boost::signals2::connection();
}
// Helper method to get the client for accessing other components
template <typename ComponentType>
ComponentType * getComponent()
{
ClNav2Z * client = nullptr;
this->requiresClient(client);
if (client)
{
return client->getComponent<ComponentType>();
}
return nullptr;
}
// NEW: Component references instead of client reference
components::CpNav2ActionInterface * nav2ActionInterface_ = nullptr;
smacc2::client_core_components::CpActionClient<nav2_msgs::action::NavigateToPose> *
actionClient_ = nullptr;
// Common legacy member for result tracking
rclcpp_action::ResultCode navigationResult_;
// REMOVED: All legacy client-based members
// ❌ cl_nav2z::ClNav2Z * nav2zClient_
// ❌ cl_nav2z::ClNav2Z::SmaccNavigateResultSignal::SharedPtr navigationCallback_
// ❌ rclcpp_action::ResultCode navigationResult_
// ❌ goalHandleFuture_
// Virtual methods for derived classes - now use component types
virtual void onNavigationResult(const components::CpNav2ActionInterface::WrappedResult &) {}
virtual void onNavigationActionSuccess(const components::CpNav2ActionInterface::WrappedResult &)
{
}
virtual void onNavigationActionAbort(const components::CpNav2ActionInterface::WrappedResult &) {}
};
enum class SpinningPlanner
{
Default,
PureSpinning,
Forward
};
} // namespace cl_nav2z

50
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/include/cl_nav2z/client_behaviors/cb_navigate_backwards.hpp vendored
View File

@@ -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 <cl_nav2z/components/odom_tracker/cp_odom_tracker.hpp>
#include <optional>
#include <cl_nav2z/components/waypoints_navigator/cp_waypoints_navigator.hpp>
#include "cb_nav2z_client_behavior_base.hpp"
namespace cl_nav2z
{
// It sends the mobile base some distance backwards
class CbNavigateBackwards : public CbNav2ZClientBehaviorBase
{
public:
float backwardDistance;
// just a stub to show how to use parameterless constructor
std::optional<float> backwardSpeed;
std::optional<std::string> goalChecker_;
cl_nav2z::odom_tracker::CpOdomTracker * odomTracker_;
CbNavigateBackwards(float backwardDistanceMeters);
void onEntry() override;
void onExit() override;
};
} // namespace cl_nav2z

79
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/include/cl_nav2z/client_behaviors/cb_navigate_forward.hpp vendored
View File

@@ -1,79 +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 <tf2/utils.h>
#include <tf2_ros/buffer.h>
#include <cl_nav2z/components/odom_tracker/cp_odom_tracker.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <optional>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include "cb_nav2z_client_behavior_base.hpp"
namespace cl_nav2z
{
struct CbNavigateForwardOptions
{
// just a stub to show how to use parameterless constructor
std::optional<float> forwardSpeed;
// this may be useful in the case you want to do a straight line with some known direction
// and the robot may not have that specific initial orientation at that moment.
// If it is not set, the orientation of the straight line is the orientation of the initial (current) state.
std::optional<geometry_msgs::msg::Quaternion> forceInitialOrientation;
// the name of the goal checker selected in the navigation2 stack
std::optional<std::string> goalChecker_;
};
// Performs a relative motion forwards
class CbNavigateForward : public CbNav2ZClientBehaviorBase
{
public:
CbNavigateForwardOptions options;
CbNavigateForward();
CbNavigateForward(float forwardDistance);
// alternative forward motion using an absolute goal position instead a relative linear distance.
// It is specially to retry a previous relative incorrect motions based on distance.
// The developer user is in charge of setting a valid goal position that is located forward
// from the current position and orientation.
CbNavigateForward(geometry_msgs::msg::PoseStamped goalPosition);
virtual ~CbNavigateForward();
void onEntry() override;
void onExit() override;
void setForwardDistance(float distance_meters);
protected:
// required component
odom_tracker::CpOdomTracker * odomTracker_;
std::optional<geometry_msgs::msg::PoseStamped> goalPose_;
std::optional<float> forwardDistance_;
};
} // namespace cl_nav2z

69
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/include/cl_nav2z/client_behaviors/cb_navigate_global_position.hpp vendored
View File

@@ -1,69 +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 <geometry_msgs/msg/point.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <nav2_msgs/action/navigate_to_pose.hpp>
#include "cb_nav2z_client_behavior_base.hpp"
namespace cl_nav2z
{
struct CbNavigateGlobalPositionOptions
{
// std::optional<float> yawTolerance;
// std::optional<float> yawToleranceX;
// std::optional<float> yawToleranceY;
std::optional<std::string> goalChecker_;
std::optional<std::string> controllerName_;
};
class CbNavigateGlobalPosition : public CbNav2ZClientBehaviorBase
{
public:
float goalYaw;
CbNavigateGlobalPositionOptions options;
geometry_msgs::msg::Point goalPosition;
CbNavigateGlobalPosition();
CbNavigateGlobalPosition(
float x, float y, float yaw /*radians*/,
std::optional<CbNavigateGlobalPositionOptions> options = std::nullopt);
void setGoal(const geometry_msgs::msg::Pose & pose);
virtual void onEntry() override;
// This is the substate destructor. This code will be executed when the
// workflow exits from this substate (that is according to statechart the moment when this object is destroyed)
void onExit() override;
// auxiliary function that defines the motion that is requested to the navigation2 action server
void execute();
private:
void readStartPoseFromParameterServer(nav2_msgs::action::NavigateToPose::Goal & goal);
};
} // namespace cl_nav2z

45
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/include/cl_nav2z/client_behaviors/cb_navigate_named_waypoint.hpp vendored
View File

@@ -1,45 +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 <cl_nav2z/cl_nav2z.hpp>
#include <cl_nav2z/components/waypoints_navigator/cp_waypoints_navigator.hpp>
#include "cb_nav2z_client_behavior_base.hpp"
#include "cb_navigate_next_waypoint.hpp"
namespace cl_nav2z
{
class CbNavigateNamedWaypoint : public CbNavigateNextWaypoint
{
public:
CbNavigateNamedWaypoint(
std::string waypointname, std::optional<NavigateNextWaypointOptions> options = std::nullopt);
virtual ~CbNavigateNamedWaypoint();
void onEntry() override;
void onExit() override;
CpWaypointNavigator * waypointsNavigator_;
std::string waypointname_;
};
} // namespace cl_nav2z

45
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/include/cl_nav2z/client_behaviors/cb_navigate_next_waypoint.hpp vendored
View File

@@ -1,45 +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 <cl_nav2z/cl_nav2z.hpp>
#include <cl_nav2z/components/waypoints_navigator/cp_waypoints_navigator.hpp>
#include "cb_nav2z_client_behavior_base.hpp"
namespace cl_nav2z
{
class CbNavigateNextWaypoint : public CbNav2ZClientBehaviorBase
{
public:
CbNavigateNextWaypoint(std::optional<NavigateNextWaypointOptions> options = std::nullopt);
virtual ~CbNavigateNextWaypoint();
void onEntry() override;
void onExit() override;
protected:
CpWaypointNavigator * waypointsNavigator_;
NavigateNextWaypointOptions options_;
};
} // namespace cl_nav2z

44
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/include/cl_nav2z/client_behaviors/cb_navigate_next_waypoint_free.hpp vendored
View File

@@ -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 <cl_nav2z/client_behaviors/cb_position_control_free_space.hpp>
#include <cl_nav2z/components/waypoints_navigator/cp_waypoints_navigator_base.hpp>
namespace cl_nav2z
{
class CbNavigateNextWaypointFree : public cl_nav2z::CbPositionControlFreeSpace
{
public:
CbNavigateNextWaypointFree();
virtual ~CbNavigateNextWaypointFree();
void onEntry() override;
void onSucessCallback();
void onExit() override;
protected:
cl_nav2z::CpWaypointNavigatorBase * waypointsNavigator_;
};
} // namespace cl_nav2z

100
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/include/cl_nav2z/client_behaviors/cb_navigate_next_waypoint_until_reached.hpp vendored
View File

@@ -1,100 +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 <cl_nav2z/components/nav2_action_interface/cp_nav2_action_interface.hpp>
#include "cb_navigate_next_waypoint.hpp"
namespace cl_nav2z
{
template <typename AsyncCB, typename Orthogonal>
struct EvGoalWaypointReached : sc::event<EvGoalWaypointReached<AsyncCB, Orthogonal>>
{
};
class CbNavigateNextWaypointUntilReached : public CbNavigateNextWaypoint
{
public:
CbNavigateNextWaypointUntilReached(
std::string goalWaypointName,
std::optional<NavigateNextWaypointOptions> options = std::nullopt);
virtual ~CbNavigateNextWaypointUntilReached();
template <typename TOrthogonal, typename TSourceObject>
void onStateOrthogonalAllocation()
{
CbNavigateNextWaypoint::onStateOrthogonalAllocation<TOrthogonal, TSourceObject>();
postEvGoalWaypointReached_ = [this]()
{ this->postEvent<EvGoalWaypointReached<TSourceObject, TOrthogonal>>(); };
}
void onEntry() override;
void onExit() override;
void onNavigationActionSuccess(
const components::CpNav2ActionInterface::WrappedResult & r) override
{
// if (!isOwnActionResponse(r))
// {
// RCLCPP_WARN(
// getLogger(), "[%s] Propagating success event skipped. Action response is not ours.",
// getName().c_str());
// return;
// }
navigationResult_ = r.code;
RCLCPP_INFO(
getLogger(), "[%s] Propagating success event from action server", getName().c_str());
this->requiresComponent(waypointsNavigator_);
auto current_waypoint_name = waypointsNavigator_->getCurrentWaypointName();
if (current_waypoint_name == this->goalWaypointName_)
{
RCLCPP_INFO(
getLogger(),
"[CbNavigateNextWaypointUntilReached] GoalReached current iteration waypoint i: %ld with "
"name '%s'",
waypointsNavigator_->getCurrentWaypointIndex(), current_waypoint_name->c_str());
this->postEvGoalWaypointReached_();
}
else
{
RCLCPP_INFO(
getLogger(),
"[CbNavigateNextWaypointUntilReached] goal:'%s' current:'%s'. keep navigating.",
goalWaypointName_.c_str(), current_waypoint_name->c_str());
}
this->postSuccessEvent();
}
private:
std::string goalWaypointName_;
std::function<void()> postEvGoalWaypointReached_;
};
} // namespace cl_nav2z

37
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/include/cl_nav2z/client_behaviors/cb_pause_slam.hpp vendored
View File

@@ -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 <cl_nav2z/components/slam_toolbox/cp_slam_toolbox.hpp>
#include <slam_toolbox/srv/pause.hpp>
#include <smacc2/client_behaviors/cb_call_service.hpp>
namespace cl_nav2z
{
class CbPauseSlam : public smacc2::client_behaviors::CbServiceCall<slam_toolbox::srv::Pause>
{
public:
CbPauseSlam(std::string serviceName = "/slam_toolbox/pause_new_measurements");
void onEntry() override;
protected:
CpSlamToolbox * slam_;
};
} // namespace cl_nav2z

64
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/include/cl_nav2z/client_behaviors/cb_position_control_free_space.hpp vendored
View File

@@ -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 <cl_nav2z/components/pose/cp_pose.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <smacc2/smacc_asynchronous_client_behavior.hpp>
namespace cl_nav2z
{
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<geometry_msgs::msg::Twist>::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 cl_nav2z

48
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/include/cl_nav2z/client_behaviors/cb_pure_spinning.hpp vendored
View File

@@ -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 <geometry_msgs/msg/twist.hpp>
#include <smacc2/smacc_asynchronous_client_behavior.hpp>
namespace cl_nav2z
{
struct CbPureSpinning : public smacc2::SmaccAsyncClientBehavior
{
private:
double targetYaw__rads;
bool goalReached_;
double k_betta_;
double max_angular_yaw_speed_;
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
public:
double yaw_goal_tolerance_rads_;
CbPureSpinning(double targetYaw_rads, double max_angular_yaw_speed = 0.5);
void updateParameters();
void onEntry() override;
void onExit() override;
};
} // namespace cl_nav2z

37
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/include/cl_nav2z/client_behaviors/cb_resume_slam.hpp vendored
View File

@@ -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 <slam_toolbox/srv/pause.hpp>
#include "cl_nav2z/components/slam_toolbox/cp_slam_toolbox.hpp"
#include "smacc2/client_behaviors/cb_call_service.hpp"
namespace cl_nav2z
{
class CbResumeSlam : public smacc2::client_behaviors::CbServiceCall<slam_toolbox::srv::Pause>
{
public:
CbResumeSlam(std::string serviceName = "/slam_toolbox/pause_new_measurements");
void onEntry() override;
protected:
CpSlamToolbox * slam_;
};
} // namespace cl_nav2z

51
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/include/cl_nav2z/client_behaviors/cb_retry_behavior.hpp vendored
View File

@@ -1,51 +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 <cl_nav2z/components/odom_tracker/cp_odom_tracker.hpp>
#include <optional>
#include "cb_nav2z_client_behavior_base.hpp"
namespace cl_nav2z
{
using odom_tracker::CpOdomTracker;
template <typename TCbRelativeMotion>
class CbRetry : public TCbRelativeMotion
{
public:
CbRetry() {}
void onEntry() override
{
odomTracker_ = this->nav2zClient_->template getComponent<CpOdomTracker>();
auto goal = odomTracker_->getCurrentMotionGoal();
if (goal)
{
this->goalPose_ = *goal;
}
TCbRelativeMotion::onEntry();
}
private:
CpOdomTracker * odomTracker_;
};
} // namespace cl_nav2z

47
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/include/cl_nav2z/client_behaviors/cb_rotate.hpp vendored
View File

@@ -1,47 +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 <tf2_ros/buffer.h>
#include "cb_nav2z_client_behavior_base.hpp"
namespace cl_nav2z
{
class CbRotate : public CbNav2ZClientBehaviorBase
{
public:
float rotateDegree;
std::optional<std::string> goalChecker_;
std::optional<cl_nav2z::SpinningPlanner> spinningPlanner;
CbRotate(float rotate_degree);
CbRotate(float rotate_degree, cl_nav2z::SpinningPlanner spinning_planner);
void onEntry() override;
private:
//TODO: replace this with the Pose Component in the same way it is done in the CbAbsoluteRotateBehavior
std::shared_ptr<tf2_ros::Buffer> listener;
};
} // namespace cl_nav2z

43
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/include/cl_nav2z/client_behaviors/cb_rotate_look_at.hpp vendored
View File

@@ -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 <tf2_ros/buffer.h>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include "cb_absolute_rotate.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
namespace cl_nav2z
{
class CbRotateLookAt : public CbAbsoluteRotate
{
public:
std::shared_ptr<tf2_ros::Buffer> listener;
std::optional<geometry_msgs::msg::PoseStamped> lookAtPose_;
CbRotateLookAt();
CbRotateLookAt(const geometry_msgs::msg::PoseStamped & lookAtPose);
void onEntry() override;
// void onExit() override;
};
} // namespace cl_nav2z

46
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/include/cl_nav2z/client_behaviors/cb_save_slam_map.hpp vendored
View File

@@ -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
*
******************************************************************************************************************/
#pragma once
#include <memory>
#include <nav2_msgs/srv/save_map.hpp>
#include <slam_toolbox/srv/save_map.hpp>
#include <smacc2/client_behaviors/cb_call_service.hpp>
#include <smacc2/smacc_asynchronous_client_behavior.hpp>
namespace cl_nav2z
{
using namespace std::chrono_literals;
template <typename TService>
using CbServiceCall = smacc2::client_behaviors::CbServiceCall<TService>;
struct CbSaveSlamMap : public CbServiceCall<nav2_msgs::srv::SaveMap>
{
public:
CbSaveSlamMap();
// void onEntry() override {}
void onExit() override;
std::shared_ptr<nav2_msgs::srv::SaveMap::Request> getRequest(
/*slam_toolbox::srv::SaveMap_Request_<std::allocator<void> >::_name_type saved_map_name*/);
};
} // namespace cl_nav2z

48
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/include/cl_nav2z/client_behaviors/cb_seek_waypoint.hpp vendored
View File

@@ -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 <cl_nav2z/cl_nav2z.hpp>
#include <cl_nav2z/components/waypoints_navigator/cp_waypoints_navigator.hpp>
#include "cb_nav2z_client_behavior_base.hpp"
namespace cl_nav2z
{
class CbSeekWaypoint : public smacc2::SmaccClientBehavior
{
public:
CbSeekWaypoint(std::string skip_until_name);
virtual ~CbSeekWaypoint();
void onEntry() override;
void onExit() override;
CpWaypointNavigator * waypointsNavigator_;
NavigateNextWaypointOptions options_;
private:
std::optional<int> count_;
std::optional<std::string> seekWaypointName_;
};
} // namespace cl_nav2z

61
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/include/cl_nav2z/client_behaviors/cb_spiral_motion.hpp vendored
View File

@@ -1,61 +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 <cl_nav2z/components/pose/cp_pose.hpp>
#include <smacc2/smacc_asynchronous_client_behavior.hpp>
namespace cl_nav2z
{
// client behaviors typically accept parameters to be customized in the state it is being used
// we follow this pattern, with a basic structure with optional fields, that make thee initialization of the behavior more readable in the
// state initialization. It also avoid an explosion of constructors for each variation of the parameters.
struct CbSpiralMotionOptions
{
std::optional<float> linearVelocity = 0.0f;
std::optional<float> maxLinearVelocity = 0.5f;
std::optional<float> initialAngularVelocity = 1.5f;
std::optional<rclcpp::Duration> spiralMotionDuration = rclcpp::Duration::from_seconds(40);
std::optional<float> finalRadius = 20.0f; //meters
};
/* a basic client behavior has a simple onEntry and onExit functions. When we enter into a stae we call onEntry.
This client behavior is asynchronous so that onEntry returns immediately, it opens a thread to run the behavior*/
struct CbSpiralMotion : public smacc2::SmaccAsyncClientBehavior
{
public:
//
// constructor, we accept an empty usag of the behavior, but also any other combination of parameters
CbSpiralMotion(std::optional<CbSpiralMotionOptions> options = std::nullopt);
void onEntry() override;
void onExit() override;
private:
// this client behavior has its own temporal publisher to control the robot
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmdVelPub_;
// we store the options that customize the behavior
CbSpiralMotionOptions options_;
};
} // namespace cl_nav2z

38
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/include/cl_nav2z/client_behaviors/cb_stop_navigation.hpp vendored
View File

@@ -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 <tf2_ros/buffer.h>
#include "cb_nav2z_client_behavior_base.hpp"
#include "cb_navigate_global_position.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
namespace cl_nav2z
{
class CbStopNavigation : public CbNavigateGlobalPosition
{
public:
CbStopNavigation();
void onEntry() override;
void onExit() override;
};
} // namespace cl_nav2z

38
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/include/cl_nav2z/client_behaviors/cb_track_path_odometry.hpp vendored
View File

@@ -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 <cl_nav2z/components/pose/cp_pose.hpp>
#include <smacc2/smacc_asynchronous_client_behavior.hpp>
namespace cl_nav2z
{
struct CbTrackPathOdometry : public smacc2::SmaccAsyncClientBehavior
{
private:
public:
CbTrackPathOdometry();
void onEntry() override;
void onExit() override;
};
} // namespace cl_nav2z

38
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/include/cl_nav2z/client_behaviors/cb_track_path_slam.hpp vendored
View File

@@ -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 <cl_nav2z/components/pose/cp_pose.hpp>
#include <smacc2/smacc_asynchronous_client_behavior.hpp>
namespace cl_nav2z
{
struct CbTrackPathSLAM : public smacc2::SmaccAsyncClientBehavior
{
private:
public:
CbTrackPathSLAM();
void onEntry() override;
void onExit() override;
};
} // namespace cl_nav2z

56
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/include/cl_nav2z/client_behaviors/cb_undo_path_backwards.hpp vendored
View File

@@ -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 <tf2_ros/buffer.h>
#include <cl_nav2z/components/odom_tracker/cp_odom_tracker.hpp>
#include "cb_nav2z_client_behavior_base.hpp"
namespace cl_nav2z
{
using ::cl_nav2z::odom_tracker::CpOdomTracker;
struct CbUndoPathBackwardsOptions
{
// the name of the goal checker selected in the navigation2 stack
std::optional<std::string> goalCheckerId_;
// the name of the goal checker selected in the navigation2 stack
std::optional<std::string> undoControllerName_;
};
class CbUndoPathBackwards : public CbNav2ZClientBehaviorBase
{
public:
CbUndoPathBackwards(std::optional<CbUndoPathBackwardsOptions> options = std::nullopt);
void onEntry() override;
void onExit() override;
private:
std::shared_ptr<tf2_ros::Buffer> listener;
CpOdomTracker * odomTracker;
std::optional<CbUndoPathBackwardsOptions> options_;
};
} // namespace cl_nav2z

67
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/include/cl_nav2z/client_behaviors/cb_wait_nav2_nodes.hpp vendored
View File

@@ -1,67 +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 <tf2_ros/buffer.h>
#include <bond/msg/status.hpp>
#include <smacc2/smacc_asynchronous_client_behavior.hpp>
namespace cl_nav2z
{
enum class Nav2Nodes
{
PlannerServer,
ControllerServer,
RecoveriesServer,
BtNavigator,
MapServer,
None
};
std::string toString(Nav2Nodes value);
Nav2Nodes fromString(std::string str);
class CbWaitNav2Nodes : public smacc2::SmaccAsyncClientBehavior
{
public:
CbWaitNav2Nodes(
std::string topicname,
std::vector<Nav2Nodes> waitNodes = {
Nav2Nodes::PlannerServer, Nav2Nodes::ControllerServer, Nav2Nodes::BtNavigator});
CbWaitNav2Nodes(
std::vector<Nav2Nodes> waitNodes = {
Nav2Nodes::PlannerServer, Nav2Nodes::ControllerServer, Nav2Nodes::BtNavigator});
void onEntry() override;
private:
void onMessageReceived(const bond::msg::Status & msg);
std::string topicname_;
rclcpp::Subscription<bond::msg::Status>::SharedPtr sub_;
std::map<Nav2Nodes, bool> receivedAliveMsg_;
std::vector<Nav2Nodes> waitNodes_;
};
} // namespace cl_nav2z

59
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/include/cl_nav2z/client_behaviors/cb_wait_pose.hpp vendored
View File

@@ -1,59 +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 <cl_nav2z/cl_nav2z.hpp>
#include <smacc2/smacc_asynchronous_client_behavior.hpp>
namespace cl_nav2z
{
// waits a robot pose message. Usually used for the startup synchronization.
enum class WaitPoseStandardReferenceFrame
{
Map,
Odometry
};
// Waits a new pose using the Pose Component
// the specific pose to wait is configured in that component
class CbWaitPose : public smacc2::SmaccAsyncClientBehavior
{
public:
// waits a new pose update of the Pose Component
CbWaitPose();
// waits a new pose update of the Pose Component in some reference frame (if there is no connection it will wait)
CbWaitPose(WaitPoseStandardReferenceFrame frame);
virtual ~CbWaitPose();
template <typename TOrthogonal, typename TSourceObject>
void onStateOrthogonalAllocation()
{
this->requiresClient(nav2zClient_);
smacc2::SmaccAsyncClientBehavior::onStateOrthogonalAllocation<TOrthogonal, TSourceObject>();
}
void onEntry() override;
protected:
cl_nav2z::ClNav2Z * nav2zClient_;
};
} // namespace cl_nav2z

60
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/include/cl_nav2z/client_behaviors/cb_wait_transform.hpp vendored
View File

@@ -1,60 +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 <cl_nav2z/cl_nav2z.hpp>
#include <smacc2/smacc_asynchronous_client_behavior.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <tf2/transform_datatypes.h>
#include <tf2/utils.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
namespace cl_nav2z
{
class CbWaitTransform : public smacc2::SmaccAsyncClientBehavior
{
public:
CbWaitTransform(std::string targetFrame, std::string referenceFrame, rclcpp::Duration timeout);
virtual ~CbWaitTransform();
template <typename TOrthogonal, typename TSourceObject>
void onStateOrthogonalAllocation()
{
smacc2::SmaccAsyncClientBehavior::onStateOrthogonalAllocation<TOrthogonal, TSourceObject>();
}
void onEntry() override;
protected:
// shared static listener
std::shared_ptr<tf2_ros::Buffer> tfBuffer_;
std::shared_ptr<tf2_ros::TransformListener> tfListener_;
std::string targetFrame_;
std::string referenceFrame_;
rclcpp::Duration timeout_;
std::optional<tf2::Stamped<tf2::Transform>> result_;
};
} // namespace cl_nav2z

34
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/include/cl_nav2z/common.hpp vendored
View File

@@ -1,34 +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.
#pragma once
#include <iostream>
#include <tf2/transform_datatypes.h>
#include <builtin_interfaces/msg/time.hpp>
#include <geometry_msgs/msg/point.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/quaternion.hpp>
#include <geometry_msgs/msg/quaternion_stamped.hpp>
#include <nav2_msgs/action/navigate_to_pose.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
std::ostream & operator<<(std::ostream & out, const geometry_msgs::msg::Quaternion & msg);
std::ostream & operator<<(std::ostream & out, const geometry_msgs::msg::Pose & msg);
std::ostream & operator<<(std::ostream & out, const geometry_msgs::msg::Point & msg);
std::ostream & operator<<(std::ostream & out, const geometry_msgs::msg::PoseStamped & msg);
std::ostream & operator<<(std::ostream & out, const nav2_msgs::action::NavigateToPose::Goal & msg);
std::ostream & operator<<(std::ostream & out, const builtin_interfaces::msg::Time & msg);

43
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/include/cl_nav2z/components/amcl/cp_amcl.hpp vendored
View File

@@ -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 <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <smacc2/smacc.hpp>
namespace cl_nav2z
{
class CpAmcl : public smacc2::ISmaccComponent
{
public:
CpAmcl();
virtual ~CpAmcl();
std::string getName() const override;
void onInitialize() override;
void setInitialPose(const geometry_msgs::msg::PoseWithCovarianceStamped & initialpose);
private:
rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr initalPosePub_;
};
} // namespace cl_nav2z

92
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/include/cl_nav2z/components/costmap_switch/cp_costmap_switch.hpp vendored
View File

@@ -1,92 +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 <array>
#include <functional>
#include <map>
#include <memory>
#include <string>
#include <rclcpp/rclcpp.hpp>
#include <cl_nav2z/cl_nav2z.hpp>
#include <smacc2/component.hpp>
// #include <dynamic_reconfigure/DoubleParameter.h>
// #include <dynamic_reconfigure/Reconfigure.h>
// #include <dynamic_reconfigure/Config.h>
namespace cl_nav2z
{
class CpCostmapProxy;
class CpCostmapSwitch : public smacc2::ISmaccComponent
{
public:
enum class StandardLayers
{
GLOBAL_OBSTACLES_LAYER = 0,
LOCAL_OBSTACLES_LAYER = 1,
GLOBAL_INFLATED_LAYER = 2,
LOCAL_INFLATED_LAYER = 3
};
static std::array<std::string, 4> layerNames;
CpCostmapSwitch();
void onInitialize() override;
static std::string getStandardCostmapName(StandardLayers layertype);
bool exists(std::string layerName);
void enable(std::string layerName);
void enable(StandardLayers layerType);
void disable(std::string layerName);
void disable(StandardLayers layerType);
void registerProxyFromDynamicReconfigureServer(
std::string costmapName, std::string enablePropertyName = "enabled");
private:
std::map<std::string, std::shared_ptr<CpCostmapProxy>> costmapProxies;
cl_nav2z::ClNav2Z * nav2zClient_;
};
//-------------------------------------------------------------------------
class CpCostmapProxy
{
public:
CpCostmapProxy(
std::string costmap_name, std::string enablePropertyName, rclcpp::Node::SharedPtr nh);
void setCostmapEnabled(bool value);
private:
std::string costmapName_;
rclcpp::Node::SharedPtr nh_;
inline rclcpp::Node::SharedPtr getNode() { return nh_; }
};
} // namespace cl_nav2z

47
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/include/cl_nav2z/components/goal_checker_switcher/cp_goal_checker_switcher.hpp vendored
View File

@@ -1,47 +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 <string>
#include <rclcpp/rclcpp.hpp>
#include <smacc2/component.hpp>
#include <std_msgs/msg/string.hpp>
namespace cl_nav2z
{
// this class is used to switch the current goal checker of the remote navigation2 stack controller
class CpGoalCheckerSwitcher : public smacc2::ISmaccComponent
{
public:
CpGoalCheckerSwitcher(
std::string goal_checker_selector_topic = "goal_checker_selector",
std::string default_goal_checker_name = "goal_checker");
void onInitialize() override;
virtual ~CpGoalCheckerSwitcher();
void setDefaultGoalChecker();
void setGoalCheckerId(std::string goal_checker_id);
private:
std::string goal_checker_selector_topic_;
std::string default_goal_checker_name_;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr goal_checker_selector_pub_;
};
} // namespace cl_nav2z

262
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/include/cl_nav2z/components/nav2_action_interface/cp_nav2_action_interface.hpp vendored
View File

@@ -1,262 +0,0 @@
// Copyright 2024 RobosoftAI Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#pragma once
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <nav2_msgs/action/navigate_to_pose.hpp>
#include <smacc2/client_core_components/cp_action_client.hpp>
#include <smacc2/component.hpp>
#include <smacc2/smacc_signal.hpp>
#include <functional>
#include <future>
namespace cl_nav2z
{
namespace components
{
class CpNav2ActionInterface : public smacc2::ISmaccComponent
{
public:
// Type aliases for convenience
using ActionType = nav2_msgs::action::NavigateToPose;
using Goal = ActionType::Goal;
using Result = ActionType::Result;
using Feedback = ActionType::Feedback;
using GoalHandle = rclcpp_action::ClientGoalHandle<ActionType>;
using WrappedResult = typename GoalHandle::WrappedResult;
using ActionClient = smacc2::client_core_components::CpActionClient<ActionType>;
// Nav2-specific signals for external subscribers
smacc2::SmaccSignal<void(const WrappedResult &)> onNavigationSucceeded_;
smacc2::SmaccSignal<void(const WrappedResult &)> onNavigationAborted_;
smacc2::SmaccSignal<void(const WrappedResult &)> onNavigationCancelled_;
smacc2::SmaccSignal<void(const Feedback &)> onNavigationFeedback_;
// Nav2-specific event posting functions (set during orthogonal allocation)
std::function<void(const WrappedResult &)> postNavigationSuccessEvent;
std::function<void(const WrappedResult &)> postNavigationAbortedEvent;
std::function<void(const WrappedResult &)> postNavigationCancelledEvent;
std::function<void(const Feedback &)> postNavigationFeedbackEvent;
CpNav2ActionInterface() = default;
virtual ~CpNav2ActionInterface() = default;
// Public API for behaviors and other components
std::shared_future<typename GoalHandle::SharedPtr> sendNavigationGoal(
const geometry_msgs::msg::PoseStamped & target)
{
if (!actionClient_)
{
RCLCPP_ERROR(getLogger(), "[CpNav2ActionInterface] Action client not available!");
throw std::runtime_error("Action client not initialized");
}
auto goal = createNavigationGoal(target);
RCLCPP_INFO_STREAM(
getLogger(), "[CpNav2ActionInterface] Sending navigation goal to: "
<< "x=" << target.pose.position.x << ", y=" << target.pose.position.y
<< ", frame=" << target.header.frame_id);
return actionClient_->sendGoal(goal);
}
std::shared_future<typename GoalHandle::SharedPtr> sendGoal(Goal & goal)
{
if (!actionClient_)
{
RCLCPP_ERROR(getLogger(), "[CpNav2ActionInterface] Action client not available!");
throw std::runtime_error("Action client not initialized");
}
return actionClient_->sendGoal(goal);
}
bool cancelNavigation()
{
if (!actionClient_)
{
RCLCPP_WARN(getLogger(), "[CpNav2ActionInterface] Action client not available for cancel!");
return false;
}
RCLCPP_INFO(getLogger(), "[CpNav2ActionInterface] Cancelling navigation goal");
return actionClient_->cancelGoal();
}
bool isNavigationServerReady() const { return actionClient_ && actionClient_->isServerReady(); }
void waitForNavigationServer()
{
if (actionClient_)
{
actionClient_->waitForServer();
}
}
// Component lifecycle
template <typename TOrthogonal, typename TSourceObject>
void onComponentInitialization()
{
// Require the underlying action client component
this->requiresComponent(actionClient_);
// Set up nav2-specific event posting functions
postNavigationSuccessEvent = [this](const WrappedResult & result)
{
auto * ev = new smacc2::default_events::EvActionSucceeded<TSourceObject, TOrthogonal>();
this->postEvent(ev);
};
postNavigationAbortedEvent = [this](const WrappedResult & result)
{
auto * ev = new smacc2::default_events::EvActionAborted<TSourceObject, TOrthogonal>();
this->postEvent(ev);
};
postNavigationCancelledEvent = [this](const WrappedResult & result)
{
auto * ev = new smacc2::default_events::EvActionCancelled<TSourceObject, TOrthogonal>();
this->postEvent(ev);
};
postNavigationFeedbackEvent = [this](const Feedback & feedback)
{
auto * ev = new smacc2::default_events::EvActionFeedback<Feedback, TOrthogonal>();
ev->feedbackMessage = feedback;
this->postEvent(ev);
};
RCLCPP_INFO(getLogger(), "[CpNav2ActionInterface] Component initialized");
}
void onInitialize() override
{
// Wire up signal connections from action client to nav2-specific signals
if (actionClient_)
{
// Connect action client signals to our nav2-specific signals and event posting using method pointers
actionClient_->onSucceeded(&CpNav2ActionInterface::onNavigationSuccess, this);
actionClient_->onAborted(&CpNav2ActionInterface::onNavigationAborted, this);
actionClient_->onCancelled(&CpNav2ActionInterface::onNavigationCancelled, this);
actionClient_->onFeedback(&CpNav2ActionInterface::onNavigationFeedback, this);
RCLCPP_INFO(getLogger(), "[CpNav2ActionInterface] Signal connections established");
}
else
{
RCLCPP_ERROR(
getLogger(), "[CpNav2ActionInterface] Action client not found during initialization!");
}
}
// Signal connection methods for external subscribers
template <typename T>
boost::signals2::connection onNavigationSucceeded(
void (T::*callback)(const WrappedResult &), T * object)
{
return this->getStateMachine()->createSignalConnection(
onNavigationSucceeded_, callback, object);
}
template <typename T>
boost::signals2::connection onNavigationAborted(
void (T::*callback)(const WrappedResult &), T * object)
{
return this->getStateMachine()->createSignalConnection(onNavigationAborted_, callback, object);
}
template <typename T>
boost::signals2::connection onNavigationCancelled(
void (T::*callback)(const WrappedResult &), T * object)
{
return this->getStateMachine()->createSignalConnection(
onNavigationCancelled_, callback, object);
}
template <typename T>
boost::signals2::connection onNavigationFeedback(
void (T::*callback)(const Feedback &), T * object)
{
return this->getStateMachine()->createSignalConnection(onNavigationFeedback_, callback, object);
}
// Access to underlying action client for advanced usage
ActionClient * getActionClient() const { return actionClient_; }
private:
ActionClient * actionClient_ = nullptr;
// Event translation callbacks
void onNavigationSuccess(const WrappedResult & result)
{
RCLCPP_INFO(getLogger(), "[CpNav2ActionInterface] Navigation succeeded");
onNavigationSucceeded_(result);
if (postNavigationSuccessEvent)
{
postNavigationSuccessEvent(result);
}
}
void onNavigationAborted(const WrappedResult & result)
{
RCLCPP_WARN(getLogger(), "[CpNav2ActionInterface] Navigation aborted");
onNavigationAborted_(result);
if (postNavigationAbortedEvent)
{
postNavigationAbortedEvent(result);
}
}
void onNavigationCancelled(const WrappedResult & result)
{
RCLCPP_INFO(getLogger(), "[CpNav2ActionInterface] Navigation cancelled");
onNavigationCancelled_(result);
if (postNavigationCancelledEvent)
{
postNavigationCancelledEvent(result);
}
}
void onNavigationFeedback(const Feedback & feedback)
{
RCLCPP_DEBUG(getLogger(), "[CpNav2ActionInterface] Navigation feedback received");
onNavigationFeedback_(feedback);
if (postNavigationFeedbackEvent)
{
postNavigationFeedbackEvent(feedback);
}
}
// Nav2-specific goal construction
Goal createNavigationGoal(const geometry_msgs::msg::PoseStamped & target)
{
Goal goal;
goal.pose = target;
// Set default behavior_tree if not specified
if (goal.behavior_tree.empty())
{
goal.behavior_tree = ""; // Let Nav2 use default behavior tree
}
return goal;
}
};
} // namespace components
} // namespace cl_nav2z

218
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/include/cl_nav2z/components/odom_tracker/cp_odom_tracker.hpp vendored
View File

@@ -1,218 +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 <tf2/transform_datatypes.h>
#include <smacc2/common.hpp>
#include <smacc2/component.hpp>
#include <rclcpp/rclcpp.hpp>
#include <memory>
#include <mutex>
#include <vector>
#include <geometry_msgs/msg/point.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <nav2_msgs/action/navigate_to_pose.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <nav_msgs/msg/path.hpp>
#include <cl_nav2z/components/pose/cp_pose.hpp>
#include <std_msgs/msg/header.hpp>
namespace cl_nav2z
{
namespace odom_tracker
{
enum class WorkingMode : uint8_t
{
RECORD_PATH = 0,
CLEAR_PATH = 1,
IDLE = 2
};
enum class OdomTrackerStrategy
{
ODOMETRY_SUBSCRIBER,
POSE_COMPONENT
};
/// This object tracks and saves the trajectories performed by the vehicle
/// so that they can be used later to execute operations such as "undo motions"
class CpOdomTracker : public smacc2::ISmaccComponent
{
public:
// by default, the component start in record_path mode and publishing the
// current path
CpOdomTracker(
std::string odomtopicName = "/odom", std::string odomFrame = "odom",
OdomTrackerStrategy strategy = OdomTrackerStrategy::ODOMETRY_SUBSCRIBER);
// threadsafe
/// odom callback: Updates the path - this must be called periodically for each odometry message.
// The odom parameters is the main input of this tracker
virtual void processNewPose(const geometry_msgs::msg::PoseStamped & odom);
virtual void odomMessageCallback(const nav_msgs::msg::Odometry::SharedPtr odom);
virtual void update();
// ------ CONTROL COMMANDS ---------------------
// threadsafe
void setWorkingMode(WorkingMode workingMode);
// threadsafe
void setPublishMessages(bool value);
// threadsafe
void pushPath();
// threadsafe
void pushPath(std::string pathname);
// threadsafe
void popPath(int pathCount = 1, bool keepPreviousPath = false);
// threadsafe
void clearPath();
// threadsafe
void setStartPoint(const geometry_msgs::msg::PoseStamped & pose);
// threadsafe
void setStartPoint(const geometry_msgs::msg::Pose & pose);
// threadsafe - use only for recording mode, it is reset always a new path is pushed, popped or cleared
void setCurrentMotionGoal(const geometry_msgs::msg::PoseStamped & pose);
void setCurrentPathName(const std::string & currentPathName);
// threadsafe
std::optional<geometry_msgs::msg::PoseStamped> getCurrentMotionGoal();
// threadsafe
nav_msgs::msg::Path getPath();
void logStateString(bool debug = true);
// parameters update callback
void updateParameters();
inline void setOdomFrame(std::string odomFrame)
{
odomFrame_ = odomFrame;
getNode()->set_parameter(rclcpp::Parameter("odom_frame", odomFrame));
}
protected:
void onInitialize() override;
void updateConfiguration();
virtual void rtPublishPaths(rclcpp::Time timestamp);
// this is called when a new odom message is received in record path mode
virtual bool updateRecordPath(const geometry_msgs::msg::PoseStamped & odom);
// this is called when a new odom message is received in clear path mode
virtual bool updateClearPath(const geometry_msgs::msg::PoseStamped & odom);
void updateAggregatedStackPath();
// -------------- OUTPUTS ---------------------
rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr robotBasePathPub_;
rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr robotBasePathStackedPub_;
// --------------- INPUTS ------------------------
// optional, this class can be used directly calling the odomProcessing method
// without any subscriber
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odomSub_;
cl_nav2z::Pose * robotPose_;
rclcpp::TimerBase::SharedPtr robotPoseTimer_;
// -------------- PARAMETERS ----------------------
/// How much distance there is between two points of the path
double recordPointDistanceThreshold_;
/// Meters
double recordAngularDistanceThreshold_;
/// rads
double clearPointDistanceThreshold_;
/// rads
double clearAngularDistanceThreshold_;
std::string odomFrame_;
std::string odomTopicName_;
// --------------- STATE ---------------
// default true
bool publishMessages;
/// Processed path for the mouth of the reel
nav_msgs::msg::Path baseTrajectory_;
WorkingMode workingMode_;
std::vector<nav_msgs::msg::Path> pathStack_;
struct PathInfo
{
std::string name;
std::optional<geometry_msgs::msg::PoseStamped> goalPose;
};
std::vector<PathInfo> pathInfos_;
nav_msgs::msg::Path aggregatedStackPathMsg_;
OdomTrackerStrategy strategy_;
std::optional<geometry_msgs::msg::PoseStamped> currentMotionGoal_;
std::string currentPathName_;
rcl_interfaces::msg::SetParametersResult parametersCallback(
const std::vector<rclcpp::Parameter> & parameters);
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_;
std::mutex m_mutex_;
};
/**
******************************************************************************************************************
* p2pDistance()
******************************************************************************************************************
*/
inline double p2pDistance(
const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2)
{
double dx = (p1.x - p2.x);
double dy = (p1.y - p2.y);
double dz = (p2.z - p2.z);
double dist = sqrt(dx * dx + dy * dy + dz * dz);
return dist;
}
} // namespace odom_tracker
} // namespace cl_nav2z

70
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/include/cl_nav2z/components/planner_switcher/cp_planner_switcher.hpp vendored
View File

@@ -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 <rclcpp/rclcpp.hpp>
#include <smacc2/client_bases/smacc_action_client.hpp>
#include <smacc2/component.hpp>
#include <std_msgs/msg/string.hpp>
namespace cl_nav2z
{
// this component is used to switch the current planner and controller interacting
// with the remote navigation2 stack nodes (bt_navigator, planner_server, controller_server)
class CpPlannerSwitcher : public smacc2::ISmaccComponent
{
public:
CpPlannerSwitcher();
void onInitialize() override;
void setDesiredGlobalPlanner(std::string);
void setDesiredController(std::string);
void commitPublish();
// STANDARD PLANNERS
void setBackwardPlanner(bool commit = true);
void setUndoPathBackwardPlanner(bool commit = true);
void setForwardPlanner(bool commit = true);
void setPureSpinningPlanner(bool commit = true);
// sets ROS defaults local and global planners
void setDefaultPlanners(bool commit = true);
private:
std::string desired_planner_;
std::string desired_controller_;
bool set_planners_mode_flag_;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr planner_selector_pub_;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr controller_selector_pub_;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr goal_checker_selector_pub_;
};
} // namespace cl_nav2z

106
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/include/cl_nav2z/components/pose/cp_pose.hpp vendored
View File

@@ -1,106 +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.
/*****************************************************************************************************************
*-2020
* Authors: Pablo Inigo Blasco, Brett Aldrich
*
******************************************************************************************************************/
#pragma once
#include <mutex>
#include <geometry_msgs/msg/pose_stamped.h>
#include <tf2/transform_datatypes.h>
#include <tf2/utils.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/msg/quaternion_stamped.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <smacc2/component.hpp>
#include <smacc2/smacc_updatable.hpp>
namespace cl_nav2z
{
enum class StandardReferenceFrames
{
Map,
Odometry
};
class Pose : public smacc2::ISmaccComponent, public smacc2::ISmaccUpdatable
{
public:
Pose(std::string poseFrameName = "base_link", std::string referenceFrame = "odom");
Pose(StandardReferenceFrames referenceFrame);
void onInitialize() override;
void update() override;
// synchronously waits a transform in the current thread
void waitTransformUpdate(rclcpp::Rate r = rclcpp::Rate(20));
inline geometry_msgs::msg::Pose toPoseMsg()
{
std::lock_guard<std::mutex> guard(m_mutex_);
return this->pose_.pose;
}
inline geometry_msgs::msg::PoseStamped toPoseStampedMsg()
{
RCLCPP_INFO_STREAM(getLogger(), "[Pose] ToPoseMsg ");
std::lock_guard<std::mutex> guard(m_mutex_);
return this->pose_;
}
// get yaw in radians
float getYaw();
float getX();
float getY();
float getZ();
inline void setReferenceFrame(std::string referenceFrame) { referenceFrame_ = referenceFrame; }
inline const std::string & getReferenceFrame() const { return referenceFrame_; }
inline const std::string & getFrameId() const { return poseFrameName_; }
bool isInitialized;
std::optional<rclcpp::Time> frozenReferenceFrameTime;
void freezeReferenceFrame()
{
frozenReferenceFrameTime = getNode()->now() - rclcpp::Duration::from_seconds(1);
}
void unfreezeReferenceFrame() { frozenReferenceFrameTime = std::nullopt; }
private:
geometry_msgs::msg::PoseStamped pose_;
static std::shared_ptr<tf2_ros::Buffer> tfBuffer_;
static std::shared_ptr<tf2_ros::TransformListener> tfListener_;
static std::mutex listenerMutex_;
std::string poseFrameName_;
std::string referenceFrame_;
std::mutex m_mutex_;
};
} // namespace cl_nav2z

50
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/include/cl_nav2z/components/slam_toolbox/cp_slam_toolbox.hpp vendored
View File

@@ -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 <rclcpp/rclcpp.hpp>
#include <smacc2/client_bases/smacc_action_client.hpp>
#include <smacc2/component.hpp>
#include <std_msgs/msg/string.hpp>
namespace cl_nav2z
{
// stores the state of the last modification of the slam node
// (blind - open loop solution because the node does not provide that information)
class CpSlamToolbox : public smacc2::ISmaccComponent
{
public:
CpSlamToolbox();
virtual ~CpSlamToolbox();
enum class SlamToolboxState
{
Resumed,
Paused
};
inline SlamToolboxState getState() { return state_; }
void toggleState();
private:
SlamToolboxState state_;
};
} // namespace cl_nav2z

1864
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/include/cl_nav2z/components/waypoints_navigator/cp_waypoints_event_dispatcher.hpp vendored
View File

File diff suppressed because it is too large Load Diff

85
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/include/cl_nav2z/components/waypoints_navigator/cp_waypoints_navigator.hpp vendored
View File

@@ -1,85 +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 <cl_nav2z/cl_nav2z.hpp>
#include <cl_nav2z/components/nav2_action_interface/cp_nav2_action_interface.hpp>
#include <cl_nav2z/components/waypoints_navigator/cp_waypoints_event_dispatcher.hpp>
#include <smacc2/smacc.hpp>
#include <cl_nav2z/components/waypoints_navigator/cp_waypoints_navigator_base.hpp>
#include <geometry_msgs/msg/pose.hpp>
namespace cl_nav2z
{
class ClNav2Z;
struct EvWaypointFinal : sc::event<EvWaypointFinal>
{
};
struct NavigateNextWaypointOptions
{
std::optional<std::string> controllerName_;
std::optional<std::string> goalCheckerName_;
};
// 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 CpWaypointNavigatorBase
{
public:
ClNav2Z * client_;
components::CpNav2ActionInterface * nav2ActionInterface_ = nullptr;
CpWaypointNavigator();
void onInitialize() override;
template <typename TOrthogonal, typename TSourceObject>
void onStateOrthogonalAllocation()
{
waypointsEventDispatcher.initialize<TSourceObject, TOrthogonal>(client_);
this->requiresComponent(nav2ActionInterface_);
}
std::optional<std::shared_future<
std::shared_ptr<rclcpp_action::ClientGoalHandle<nav2_msgs::action::NavigateToPose> > > >
sendNextGoal(std::optional<NavigateNextWaypointOptions> options = std::nullopt);
void stopWaitingResult();
smacc2::SmaccSignal<void()> onNavigationRequestSucceded;
smacc2::SmaccSignal<void()> onNavigationRequestAborted;
smacc2::SmaccSignal<void()> onNavigationRequestCancelled;
private:
void onNavigationResult(const components::CpNav2ActionInterface::WrappedResult & r);
void onGoalReached(const components::CpNav2ActionInterface::WrappedResult & res);
void onGoalCancelled(const components::CpNav2ActionInterface::WrappedResult & /*res*/);
void onGoalAborted(const components::CpNav2ActionInterface::WrappedResult & /*res*/);
boost::signals2::connection succeddedNav2ZClientConnection_;
boost::signals2::connection abortedNav2ZClientConnection_;
boost::signals2::connection cancelledNav2ZClientConnection_;
};
} // namespace cl_nav2z

116
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/include/cl_nav2z/components/waypoints_navigator/cp_waypoints_navigator_base.hpp vendored
View File

@@ -1,116 +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 <cl_nav2z/cl_nav2z.hpp>
#include <cl_nav2z/components/waypoints_navigator/cp_waypoints_event_dispatcher.hpp>
#include <smacc2/smacc.hpp>
#include <geometry_msgs/msg/pose.hpp>
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;
// DEPRECATED: For third-party compatibility only. Third-party developers should migrate to onStateOrthogonalAllocation
// This method exists to support existing third-party classes that inherit from this base class
// and call CpWaypointNavigatorBase::onOrthogonalAllocation<TOrthogonal, TSourceObject>()
template <typename TOrthogonal, typename TSourceObject>
[[deprecated(
"Use onStateOrthogonalAllocation instead. This method exists only for third-party "
"compatibility.")]] void
onOrthogonalAllocation()
{
// Call the new method to maintain functionality for third-party inheritors
onStateOrthogonalAllocation<TOrthogonal, TSourceObject>();
}
template <typename TOrthogonal, typename TSourceObject>
void onStateOrthogonalAllocation()
{
ClNav2Z * client = dynamic_cast<ClNav2Z *>(owner_);
waypointsEventDispatcher.initialize<TSourceObject, TOrthogonal>(client);
}
void loadWayPointsFromFile(std::string filepath);
void loadWayPointsFromFile2(std::string filepath);
void setWaypoints(const std::vector<geometry_msgs::msg::Pose> & waypoints);
void setWaypoints(const std::vector<Pose2D> & waypoints);
const std::vector<geometry_msgs::msg::Pose> & getWaypoints() const;
const std::vector<std::string> & getWaypointNames() const;
std::optional<geometry_msgs::msg::Pose> getNamedPose(std::string name) const;
geometry_msgs::msg::Pose getPose(int index) const;
geometry_msgs::msg::Pose getCurrentPose() const;
long getCurrentWaypointIndex() const;
std::optional<std::string> 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);
void notifyGoalReached();
protected:
void insertWaypoint(int index, geometry_msgs::msg::Pose & newpose);
void removeWaypoint(int index);
std::vector<geometry_msgs::msg::Pose> waypoints_;
std::vector<std::string> waypointsNames_;
};
} // namespace cl_nav2z

58
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/include/cl_nav2z/components/waypoints_navigator/cp_waypoints_visualizer.hpp vendored
View File

@@ -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 <cl_nav2z/cl_nav2z.hpp>
#include <cl_nav2z/components/waypoints_navigator/cp_waypoints_navigator.hpp>
#include <smacc2/smacc.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <visualization_msgs/msg/marker_array.hpp>
namespace cl_nav2z
{
class ClNav2Z;
class CpWaypointsVisualizer : public smacc2::ISmaccComponent, public smacc2::ISmaccUpdatable
{
public:
cl_nav2z::CpWaypointNavigator * waypointsNavigator_;
CpWaypointsVisualizer(rclcpp::Duration duration);
void onInitialize() override;
protected:
virtual void update() override;
private:
std::mutex m_mutex_;
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr markersPub_;
visualization_msgs::msg::MarkerArray markers_;
visualization_msgs::msg::MarkerArray markerLabels_;
void createMarker(const geometry_msgs::msg::Pose & waypoint, visualization_msgs::msg::Marker & m);
void createMarkerLabel(
const geometry_msgs::msg::Pose & waypoint, std::string label,
visualization_msgs::msg::Marker & m);
};
} // namespace cl_nav2z

37
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/package.xml vendored
View File

@@ -1,37 +0,0 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>cl_nav2z</name>
<version>2.3.19</version>
<description>The cl_nav2z package implements SMACC Action Client Plugin for the ROS Navigation State - move_base node. Developed by Reel Robotics.</description>
<author email="pablo@ibrobotics.com">Pablo Inigo Blasco</author>
<maintainer email="pablo@ibrobotics.com">Pablo Inigo Blasco</maintainer>
<license>BSDv3</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>smacc2</depend>
<depend>slam_toolbox</depend>
<depend>angles</depend>
<depend>yaml_cpp_vendor</depend>
<depend>nav2_msgs</depend>
<depend>pluginlib</depend>
<depend>rclcpp_action</depend>
<depend>nav2_util</depend>
<depend>std_msgs</depend>
<depend>std_srvs</depend>
<depend>tf2_ros</depend>
<depend>tf2_geometry_msgs</depend>
<depend>bond</depend>
<depend>ament_index_cpp</depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

105
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/scripts/lidar_completion.py vendored
View File

@@ -1,105 +0,0 @@
# Copyright 2024 RobosoftAI Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import LaserScan
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
import logging
import copy
class LidarProcessor(Node):
def __init__(self):
super().__init__("lidar_processor")
qos_prof_1 = QoSProfile(
reliability=ReliabilityPolicy.BEST_EFFORT, history=HistoryPolicy.KEEP_LAST, depth=10
)
self.subscription = self.create_subscription(
LaserScan, "/scan_input", self.lidar_callback, qos_profile=qos_prof_1
)
qos_prof_2 = QoSProfile(
reliability=ReliabilityPolicy.RELIABLE, history=HistoryPolicy.KEEP_LAST, depth=10
)
self.msg_buffer = []
self.publisher = self.create_publisher(LaserScan, "/scan_output", qos_profile=qos_prof_2)
self.logger = self.get_logger()
self.logger.set_level(logging.INFO)
def lidar_callback(self, msg):
# self.logger.info("Received message")
self.msg_buffer.append(msg)
if len(self.msg_buffer) > 5:
# self.logger.info("Merging messages")
merged_msg = self.merge_lidar_msgs()
# self.msg_buffer.pop(0)
self.msg_buffer = []
self.publisher.publish(merged_msg)
def merge_lidar_msgs(self):
merged_ranges = [0.0] * len(self.msg_buffer[0].ranges) * len(self.msg_buffer)
# cont_inf = 0
cont_tot = 0
for i in range(len(self.msg_buffer)):
for j in range(len(self.msg_buffer[i].ranges)):
# if self.msg_buffer[i].ranges[j] != float("inf"):
merged_ranges[cont_tot] = self.msg_buffer[i].ranges[j]
cont_tot += 1
# else:
# cont_inf += 1
# self.logger.info(f"Number of infinite values from total: {cont_inf}/{cont_tot}")
merged_msg = LaserScan()
# merged_msg = copy.deepcopy(self.msg_buffer[-1])
merged_msg.header = self.msg_buffer[-1].header
merged_msg.time_increment = self.msg_buffer[0].time_increment
merged_msg.angle_increment = self.msg_buffer[0].angle_increment
merged_msg.angle_min = self.msg_buffer[0].angle_min
merged_msg.angle_max = (
self.msg_buffer[0].angle_min + (len(merged_ranges) - 1) * merged_msg.angle_increment
)
# print(f"first angle_min: {self.msg_buffer[0].angle_min}")
# print(f"angle_min: {merged_msg.angle_min}")
# print(f"angle_max: {merged_msg.angle_max}")
# merged_msg.angle_min = self.msg_buffer[0].angle_min - self.msg_buffer[0].angle_increment*len(self.msg_buffer)
# merged_msg.angle_max = merged_msg.angle_min + (len(merged_ranges)-1)*merged_msg.angle_increment
merged_msg.range_min = min(msg.range_min for msg in self.msg_buffer)
merged_msg.range_max = max(msg.range_max for msg in self.msg_buffer)
merged_msg.ranges = merged_ranges
return merged_msg
def main(args=None):
rclpy.init(args=args)
lidar_processor = LidarProcessor()
rclpy.spin(lidar_processor)
lidar_processor.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
main()

27
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/src/cl_nav2z/cl_nav2z.cpp vendored
View File

@@ -1,27 +0,0 @@
// Copyright 2024 Robosoft Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT 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 <cl_nav2z/cl_nav2z.hpp>
#include <pluginlib/class_list_macros.hpp>
namespace cl_nav2z
{
// No implementation needed for the refactored client - all functionality
// is provided through components and the header-only interface
} // namespace cl_nav2z
// Export the ClNav2Z class as type smacc2::ISmaccClient as an implementation of the ISmaccClient interface.
PLUGINLIB_EXPORT_CLASS(cl_nav2z::ClNav2Z, smacc2::ISmaccClient)

44
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/src/cl_nav2z/client_behaviors/cb_abort_navigation.cpp vendored
View File

@@ -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
*
******************************************************************************************************************/
#include <cl_nav2z/cl_nav2z.hpp>
#include <cl_nav2z/client_behaviors/cb_abort_navigation.hpp>
#include <cl_nav2z/common.hpp>
#include <cl_nav2z/components/goal_checker_switcher/cp_goal_checker_switcher.hpp>
#include <cl_nav2z/components/odom_tracker/cp_odom_tracker.hpp>
#include <cl_nav2z/components/pose/cp_pose.hpp>
#include <rclcpp/parameter_client.hpp>
namespace cl_nav2z
{
CbAbortNavigation::CbAbortNavigation() {}
void CbAbortNavigation::onEntry()
{
// this->sendGoal(goal);
this->nav2ActionInterface_->cancelNavigation();
this->postSuccessEvent();
}
void CbAbortNavigation::onExit() {}
} // namespace cl_nav2z

270
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/src/cl_nav2z/client_behaviors/cb_absolute_rotate.cpp vendored
View File

@@ -1,270 +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 <cl_nav2z/cl_nav2z.hpp>
#include <cl_nav2z/client_behaviors/cb_absolute_rotate.hpp>
#include <cl_nav2z/common.hpp>
#include <cl_nav2z/components/goal_checker_switcher/cp_goal_checker_switcher.hpp>
#include <cl_nav2z/components/odom_tracker/cp_odom_tracker.hpp>
#include <cl_nav2z/components/pose/cp_pose.hpp>
#include <rclcpp/parameter_client.hpp>
namespace cl_nav2z
{
CbAbsoluteRotate::CbAbsoluteRotate() {}
CbAbsoluteRotate::CbAbsoluteRotate(float absoluteGoalAngleDegree, float yaw_goal_tolerance)
{
this->absoluteGoalAngleDegree = absoluteGoalAngleDegree;
if (yaw_goal_tolerance >= 0)
{
this->yawGoalTolerance = yaw_goal_tolerance;
}
}
void CbAbsoluteRotate::onEntry()
{
listener = std::make_shared<tf2_ros::Buffer>(getNode()->get_clock());
double goal_angle = this->absoluteGoalAngleDegree;
RCLCPP_INFO_STREAM(getLogger(), "[CbAbsoluteRotate] Absolute yaw Angle:" << goal_angle);
auto plannerSwitcher = this->getComponent<CpPlannerSwitcher>();
// this should work better with a coroutine and await
// this->plannerSwitcher_->setForwardPlanner();
if (spinningPlanner && *spinningPlanner == SpinningPlanner::PureSpinning)
{
plannerSwitcher->setPureSpinningPlanner();
}
else
{
plannerSwitcher->setDefaultPlanners();
}
updateTemporalBehaviorParameters(false);
auto p = this->getComponent<cl_nav2z::Pose>();
auto referenceFrame = p->getReferenceFrame();
auto currentPoseMsg = p->toPoseMsg();
nav2_msgs::action::NavigateToPose::Goal goal;
goal.pose.header.frame_id = referenceFrame;
//goal.pose.header.stamp = getNode()->now();
auto targetAngle = goal_angle * M_PI / 180.0;
goal.pose.pose.position = currentPoseMsg.position;
tf2::Quaternion q;
q.setRPY(0, 0, targetAngle);
goal.pose.pose.orientation = tf2::toMsg(q);
auto odomTracker_ = this->getComponent<odom_tracker::CpOdomTracker>();
if (odomTracker_ != nullptr)
{
auto pathname = this->getCurrentState()->getName() + " - " + getName();
odomTracker_->pushPath(pathname);
odomTracker_->setStartPoint(p->toPoseStampedMsg());
odomTracker_->setCurrentMotionGoal(goal.pose);
odomTracker_->setWorkingMode(odom_tracker::WorkingMode::RECORD_PATH);
}
auto goalCheckerSwitcher = this->getComponent<CpGoalCheckerSwitcher>();
goalCheckerSwitcher->setGoalCheckerId("absolute_rotate_goal_checker");
RCLCPP_INFO_STREAM(
getLogger(),
"[" << getName() << "] current pose yaw: " << tf2::getYaw(currentPoseMsg.orientation));
RCLCPP_INFO_STREAM(
getLogger(),
"[" << getName() << "] goal pose yaw: " << tf2::getYaw(goal.pose.pose.orientation));
this->sendGoal(goal);
}
void CbAbsoluteRotate::updateTemporalBehaviorParameters(bool undo)
{
auto log = this->getLogger();
std::string nodename = "/controller_server";
auto parameters_client =
std::make_shared<rclcpp::AsyncParametersClient>(this->getNode(), nodename);
RCLCPP_INFO_STREAM(
log, "[" << getName() << "] using a parameter client to update some controller parameters: "
<< nodename << ". Waiting service.");
parameters_client->wait_for_service();
RCLCPP_INFO_STREAM(log, "[" << getName() << "] Service found: " << nodename << ".");
std::string localPlannerName;
std::vector<rclcpp::Parameter> parameters;
// dynamic_reconfigure::DoubleParameter yaw_goal_tolerance;
rclcpp::Parameter yaw_goal_tolerance("goal_checker.yaw_goal_tolerance");
rclcpp::Parameter max_vel_theta, min_vel_theta;
bool isRosBasePlanner = !spinningPlanner || *spinningPlanner == SpinningPlanner::Default;
bool isPureSpinningPlanner = spinningPlanner && *spinningPlanner == SpinningPlanner::PureSpinning;
// SELECTING CONTROLLER AND PARAMETERS NAME
if (isPureSpinningPlanner)
{
localPlannerName = "PureSpinningLocalPlanner";
max_vel_theta = rclcpp::Parameter(localPlannerName + ".max_angular_z_speed");
min_vel_theta = rclcpp::Parameter(localPlannerName + ".min_vel_theta");
}
else if (isRosBasePlanner)
{
localPlannerName = "FollowPath";
max_vel_theta = rclcpp::Parameter(localPlannerName + ".max_vel_theta");
min_vel_theta = rclcpp::Parameter(localPlannerName + ".min_vel_theta");
}
if (!undo)
{
if (yawGoalTolerance)
{
// save old yaw tolerance
auto fut = parameters_client->get_parameters(
{localPlannerName + ".yaw_goal_tolerance"},
[&](auto futureParameters)
{
auto params = futureParameters.get();
oldYawTolerance = params[0].as_double();
});
// make synchronous
fut.get();
yaw_goal_tolerance = rclcpp::Parameter("goal_checker.yaw_goal_tolerance", *yawGoalTolerance);
parameters.push_back(yaw_goal_tolerance);
RCLCPP_INFO_STREAM(
getLogger(), "[" << getName()
<< "] updating yaw tolerance local planner to: " << *yawGoalTolerance
<< ", from previous value: " << this->oldYawTolerance);
}
if (maxVelTheta)
{
if (isRosBasePlanner)
{
// nh.getParam(nodename + "/" + localPlannerName+"/min_vel_theta", oldMinVelTheta);
// getCurrentState()->getParam(nodename + "/" + localPlannerName + "/max_vel_theta", oldMaxVelTheta);
// save old yaw tolerance
auto fut = parameters_client->get_parameters(
{localPlannerName + ".max_vel_theta"},
[&](auto futureParameters)
{
auto params = futureParameters.get();
oldMaxVelTheta = params[0].as_double();
});
// make synchronous
fut.get();
}
max_vel_theta = rclcpp::Parameter(localPlannerName + ".max_vel_theta", *maxVelTheta);
min_vel_theta = rclcpp::Parameter(localPlannerName + ".min_vel_theta", -*maxVelTheta);
parameters.push_back(max_vel_theta);
parameters.push_back(min_vel_theta);
RCLCPP_INFO_STREAM(
log, "[" << getName() << "] updating max vel theta local planner to: " << *maxVelTheta
<< ", from previous value: " << this->oldMaxVelTheta);
RCLCPP_INFO_STREAM(
log, "[" << getName() << "] updating min vel theta local planner to: " << -*maxVelTheta
<< ", from previous value: " << this->oldMinVelTheta);
}
}
else
{
if (yawGoalTolerance)
{
yaw_goal_tolerance = rclcpp::Parameter("goal_checker.yaw_goal_tolerance", oldYawTolerance);
RCLCPP_INFO_STREAM(
log, "[" << getName() << "] restoring yaw tolerance local planner from: "
<< *yawGoalTolerance << " , to previous value: " << this->oldYawTolerance);
}
if (maxVelTheta)
{
if (isRosBasePlanner)
{
max_vel_theta = rclcpp::Parameter(localPlannerName + ".max_vel_theta", oldMaxVelTheta);
min_vel_theta = rclcpp::Parameter(localPlannerName + ".max_vel_theta", oldMinVelTheta);
}
parameters.push_back(max_vel_theta);
parameters.push_back(min_vel_theta);
RCLCPP_INFO_STREAM(
log, "[" << getName() << "] restoring max vel theta local planner from: " << *maxVelTheta
<< ", to previous value: " << this->oldMaxVelTheta);
RCLCPP_INFO_STREAM(
log, "[" << getName() << "] restoring min vel theta local planner from: " << -(*maxVelTheta)
<< ", to previous value: " << this->oldMinVelTheta);
}
}
if (parameters.size())
{
std::stringstream ss;
ss << "Executing asynchronous request. Parameters to update: " << std::endl;
for (auto & p : parameters)
{
ss << p.get_name() << " - " << p.value_to_string() << std::endl;
}
RCLCPP_INFO_STREAM(getLogger(), "[CbAbsoluteRotate] " << ss.str());
auto futureResults = parameters_client->set_parameters(parameters);
int i = 0;
for (auto & res : futureResults.get())
{
RCLCPP_INFO_STREAM(
getLogger(), "[" << getName() << "] parameter result: " << parameters[i].get_name() << "="
<< parameters[i].as_string() << ". Result: " << res.successful);
i++;
}
RCLCPP_INFO_STREAM(log, "[" << getName() << "] parameters updated");
}
else
{
RCLCPP_INFO_STREAM(log, "[" << getName() << "] skipping parameters update");
}
}
void CbAbsoluteRotate::onExit()
{
if (spinningPlanner && *spinningPlanner == SpinningPlanner::PureSpinning)
{
}
else
{
}
this->updateTemporalBehaviorParameters(true);
}
} // namespace cl_nav2z

53
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/src/cl_nav2z/client_behaviors/cb_active_stop.cpp vendored
View File

@@ -1,53 +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 <angles/angles.h>
#include <geometry_msgs/msg/twist.hpp>
#include <smacc2/smacc_asynchronous_client_behavior.hpp>
#include <cl_nav2z/client_behaviors/cb_active_stop.hpp>
namespace cl_nav2z
{
CbActiveStop::CbActiveStop() {}
void CbActiveStop::onEntry()
{
auto nh = this->getNode();
cmd_vel_pub_ = nh->create_publisher<geometry_msgs::msg::Twist>("/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 cl_nav2z

57
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/src/cl_nav2z/client_behaviors/cb_load_waypoints_file.cpp vendored
View File

@@ -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 <cl_nav2z/client_behaviors/cb_load_waypoints_file.hpp>
#include <cl_nav2z/components/waypoints_navigator/cp_waypoints_navigator_base.hpp>
#include <smacc2/smacc_client_behavior.hpp>
namespace cl_nav2z
{
CbLoadWaypointsFile::CbLoadWaypointsFile(std::string filepath) : filepath_(filepath) {}
CbLoadWaypointsFile::CbLoadWaypointsFile(std::string parameter_name, std::string packagenamespace)
: parameterName_(parameter_name), packageNamespace_(packagenamespace)
{
}
void CbLoadWaypointsFile::onEntry()
{
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(), packageNamespace_.value());
}
// change this to skip some points of the yaml file, default = 0
waypointsNavigator_->currentWaypoint_ = 0;
this->postSuccessEvent();
}
void CbLoadWaypointsFile::onExit() {}
} // namespace cl_nav2z

34
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/src/cl_nav2z/client_behaviors/cb_nav2z_client_behavior_base.cpp vendored
View File

@@ -1,34 +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 <cl_nav2z/client_behaviors/cb_nav2z_client_behavior_base.hpp>
#include <cl_nav2z/common.hpp>
namespace cl_nav2z
{
CbNav2ZClientBehaviorBase::~CbNav2ZClientBehaviorBase() {}
// REMOVED: All legacy implementation - behaviors now use component APIs directly
// The base class provides component access but no complex logic
// Individual behaviors should use:
// nav2ActionInterface_->sendGoal(goal)
// nav2ActionInterface_->onNavigationSucceeded(&Behavior::onSuccess, this)
// etc.
} // namespace cl_nav2z

100
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/src/cl_nav2z/client_behaviors/cb_navigate_backward.cpp vendored
View File

@@ -1,100 +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 <tf2/utils.h>
#include <cl_nav2z/client_behaviors/cb_navigate_backwards.hpp>
#include <cl_nav2z/common.hpp>
#include <cl_nav2z/components/goal_checker_switcher/cp_goal_checker_switcher.hpp>
#include <cl_nav2z/components/odom_tracker/cp_odom_tracker.hpp>
#include <cl_nav2z/components/pose/cp_pose.hpp>
#include <geometry_msgs/msg/quaternion_stamped.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
namespace cl_nav2z
{
using namespace ::cl_nav2z::odom_tracker;
CbNavigateBackwards::CbNavigateBackwards(float backwardDistance)
{
if (backwardDistance < 0)
{
RCLCPP_ERROR(getLogger(), "[CbNavigateBackwards] distance must be greater or equal than 0");
this->backwardDistance = 0;
}
this->backwardDistance = backwardDistance;
}
void CbNavigateBackwards::onEntry()
{
// straight motion distance
double dist = backwardDistance;
RCLCPP_INFO_STREAM(
getLogger(), "[CbNavigateBackwards] Straight backwards motion distance: " << dist);
auto p = this->getComponent<cl_nav2z::Pose>();
auto referenceFrame = p->getReferenceFrame();
auto currentPoseMsg = p->toPoseMsg();
tf2::Transform currentPose;
tf2::fromMsg(currentPoseMsg, currentPose);
tf2::Transform backwardDeltaTransform;
backwardDeltaTransform.setIdentity();
backwardDeltaTransform.setOrigin(tf2::Vector3(-dist, 0, 0));
tf2::Transform targetPose = currentPose * backwardDeltaTransform;
nav2_msgs::action::NavigateToPose::Goal goal;
goal.pose.header.frame_id = referenceFrame;
//goal.pose.header.stamp = getNode()->now();
tf2::toMsg(targetPose, goal.pose.pose);
RCLCPP_INFO_STREAM(getLogger(), "[CbNavigateBackwards] TARGET POSE BACKWARDS: " << goal.pose);
geometry_msgs::msg::PoseStamped currentStampedPoseMsg;
currentStampedPoseMsg.header.frame_id = referenceFrame;
currentStampedPoseMsg.header.stamp = getNode()->now();
tf2::toMsg(currentPose, currentStampedPoseMsg.pose);
odomTracker_ = this->getComponent<CpOdomTracker>();
if (odomTracker_ != nullptr)
{
this->odomTracker_->clearPath();
this->odomTracker_->setStartPoint(currentStampedPoseMsg);
this->odomTracker_->setWorkingMode(WorkingMode::RECORD_PATH);
}
auto plannerSwitcher = this->getComponent<CpPlannerSwitcher>();
plannerSwitcher->setBackwardPlanner();
auto goalCheckerSwitcher = this->getComponent<CpGoalCheckerSwitcher>();
goalCheckerSwitcher->setGoalCheckerId("backward_goal_checker");
this->sendGoal(goal);
}
void CbNavigateBackwards::onExit()
{
if (odomTracker_)
{
this->odomTracker_->setWorkingMode(WorkingMode::IDLE);
}
}
} // namespace cl_nav2z

158
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/src/cl_nav2z/client_behaviors/cb_navigate_forward.cpp vendored
View File

@@ -1,158 +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 <cl_nav2z/common.hpp>
#include <cl_nav2z/client_behaviors/cb_navigate_forward.hpp>
#include <cl_nav2z/components/goal_checker_switcher/cp_goal_checker_switcher.hpp>
#include <cl_nav2z/components/odom_tracker/cp_odom_tracker.hpp>
#include <cl_nav2z/components/pose/cp_pose.hpp>
namespace cl_nav2z
{
using ::cl_nav2z::odom_tracker::CpOdomTracker;
using ::cl_nav2z::odom_tracker::WorkingMode;
using ::cl_nav2z::Pose;
CbNavigateForward::CbNavigateForward(float distance_meters) : forwardDistance_(distance_meters) {}
CbNavigateForward::CbNavigateForward() {}
CbNavigateForward::CbNavigateForward(geometry_msgs::msg::PoseStamped goal) : goalPose_(goal) {}
CbNavigateForward::~CbNavigateForward() {}
void CbNavigateForward::setForwardDistance(float distance_meters)
{
if (distance_meters < 0)
{
RCLCPP_INFO_STREAM(
getLogger(), "[" << getName() << "] negative forward distance: " << distance_meters
<< ". Resetting to 0.");
distance_meters = 0;
}
forwardDistance_ = distance_meters;
RCLCPP_INFO_STREAM(
getLogger(), "[" << getName() << "] setting fw motion distance: " << *forwardDistance_);
}
void CbNavigateForward::onEntry()
{
if (forwardDistance_)
{
setForwardDistance(*forwardDistance_);
RCLCPP_INFO_STREAM(
getLogger(), "[" << getName() << "] Straight motion distance: " << *forwardDistance_);
}
// get current pose
auto p = this->getComponent<Pose>();
auto referenceFrame = p->getReferenceFrame();
auto currentPoseMsg = p->toPoseMsg();
RCLCPP_INFO_STREAM(
getLogger(), "[" << getName() << "]"
<< "current pose: " << currentPoseMsg);
// force global orientation if it is requested
if (options.forceInitialOrientation)
{
currentPoseMsg.orientation = *(options.forceInitialOrientation);
RCLCPP_WARN_STREAM(
getLogger(),
"[" << getName() << "]"
<< "Forcing initial straight motion orientation: " << currentPoseMsg.orientation);
}
tf2::Transform currentPose;
tf2::fromMsg(currentPoseMsg, currentPose);
tf2::Transform targetPose;
if (goalPose_)
{
tf2::fromMsg(goalPose_->pose, targetPose);
}
else if (forwardDistance_)
{
// compute forward goal pose
tf2::Transform forwardDeltaTransform;
forwardDeltaTransform.setIdentity();
forwardDeltaTransform.setOrigin(tf2::Vector3(*forwardDistance_, 0, 0));
targetPose = currentPose * forwardDeltaTransform;
}
else
{
RCLCPP_WARN_STREAM(
getLogger(),
"[" << getName() << "]"
<< "No goal Pose or Distance is specified. Aborting. No action request is sent."
<< currentPoseMsg.orientation);
return;
}
// action goal
nav2_msgs::action::NavigateToPose::Goal goal;
goal.pose.header.frame_id = referenceFrame;
//goal.pose.header.stamp = getNode()->now();
tf2::toMsg(targetPose, goal.pose.pose);
RCLCPP_INFO_STREAM(
getLogger(), "[" << getName() << "]"
<< " TARGET POSE FORWARD: " << goal.pose.pose);
// current pose
geometry_msgs::msg::PoseStamped currentStampedPoseMsg;
currentStampedPoseMsg.header.frame_id = referenceFrame;
currentStampedPoseMsg.header.stamp =
getNode()->now(); // probably it is better avoid setting that goal timestamp
tf2::toMsg(currentPose, currentStampedPoseMsg.pose);
odomTracker_ = this->getComponent<CpOdomTracker>();
if (odomTracker_ != nullptr)
{
auto pathname = this->getCurrentState()->getName() + " - " + getName();
odomTracker_->pushPath(pathname);
odomTracker_->setStartPoint(currentStampedPoseMsg);
odomTracker_->setCurrentMotionGoal(goal.pose);
odomTracker_->setWorkingMode(WorkingMode::RECORD_PATH);
}
auto plannerSwitcher = this->getComponent<CpPlannerSwitcher>();
plannerSwitcher->setForwardPlanner();
auto goalCheckerSwitcher = this->getComponent<CpGoalCheckerSwitcher>();
goalCheckerSwitcher->setGoalCheckerId("forward_goal_checker");
this->sendGoal(goal);
}
void CbNavigateForward::onExit()
{
if (odomTracker_)
{
this->odomTracker_->setWorkingMode(WorkingMode::IDLE);
}
}
} // namespace cl_nav2z

109
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/src/cl_nav2z/client_behaviors/cb_navigate_global_position.cpp vendored
View File

@@ -1,109 +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 <cl_nav2z/common.hpp>
#include <cl_nav2z/client_behaviors/cb_navigate_global_position.hpp>
#include <cl_nav2z/components/goal_checker_switcher/cp_goal_checker_switcher.hpp>
#include <cl_nav2z/components/odom_tracker/cp_odom_tracker.hpp>
#include <cl_nav2z/components/planner_switcher/cp_planner_switcher.hpp>
#include <cl_nav2z/components/pose/cp_pose.hpp>
namespace cl_nav2z
{
using namespace ::cl_nav2z::odom_tracker;
CbNavigateGlobalPosition::CbNavigateGlobalPosition() {}
CbNavigateGlobalPosition::CbNavigateGlobalPosition(
float x, float y, float yaw, std::optional<CbNavigateGlobalPositionOptions> options)
{
auto p = geometry_msgs::msg::Point();
p.x = x;
p.y = y;
goalPosition = p;
goalYaw = yaw;
if (options) this->options = *options;
}
void CbNavigateGlobalPosition::setGoal(const geometry_msgs::msg::Pose & pose)
{
goalPosition = pose.position;
goalYaw = tf2::getYaw(pose.orientation);
}
void CbNavigateGlobalPosition::onEntry()
{
RCLCPP_INFO(getLogger(), "Entering Navigate Global position");
RCLCPP_INFO(getLogger(), "Component requirements completed");
auto pose = this->getComponent<cl_nav2z::Pose>()->toPoseMsg();
auto * odomTracker = this->getComponent<CpOdomTracker>();
auto plannerSwitcher = this->getComponent<CpPlannerSwitcher>();
plannerSwitcher->setDefaultPlanners(false);
if (options.controllerName_)
{
plannerSwitcher->setDesiredController(*(options.controllerName_));
}
plannerSwitcher->commitPublish();
auto goalCheckerSwitcher = this->getComponent<CpGoalCheckerSwitcher>();
goalCheckerSwitcher->setGoalCheckerId("goal_checker");
auto pathname = this->getCurrentState()->getName() + " - " + getName();
odomTracker->pushPath(pathname);
odomTracker->setStartPoint(pose);
odomTracker->setWorkingMode(WorkingMode::RECORD_PATH);
execute();
}
// auxiliary function that defines the motion that is requested to the nav2 action server
void CbNavigateGlobalPosition::execute()
{
auto p = this->getComponent<cl_nav2z::Pose>();
auto referenceFrame = p->getReferenceFrame();
// auto currentPoseMsg = p->toPoseMsg();
RCLCPP_INFO(getLogger(), "Sending Goal to MoveBase");
nav2_msgs::action::NavigateToPose::Goal goal;
goal.pose.header.frame_id = referenceFrame;
//goal.pose.header.stamp = getNode()->now();
goal.pose.pose.position = goalPosition;
tf2::Quaternion q;
q.setRPY(0, 0, goalYaw);
goal.pose.pose.orientation = tf2::toMsg(q);
this->sendGoal(goal);
}
// This is the substate destructor. This code will be executed when the
// workflow exits from this substate (that is according to statechart the moment when this object is destroyed)
void CbNavigateGlobalPosition::onExit()
{
RCLCPP_INFO(getLogger(), "Exiting move goal Action Client");
}
} // namespace cl_nav2z

59
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/src/cl_nav2z/client_behaviors/cb_navigate_named_waypoint.cpp vendored
View File

@@ -1,59 +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 <cl_nav2z/client_behaviors/cb_navigate_named_waypoint.hpp>
namespace cl_nav2z
{
CbNavigateNamedWaypoint::CbNavigateNamedWaypoint(
std::string waypointname, std::optional<NavigateNextWaypointOptions> options)
: CbNavigateNextWaypoint(options)
{
waypointname_ = waypointname;
}
CbNavigateNamedWaypoint::~CbNavigateNamedWaypoint() {}
void CbNavigateNamedWaypoint::onEntry()
{
// waypointsNavigator_ = this->getComponent<CpWaypointNavigator>();
// waypointsNavigator_->sendNextGoal(options_);
// auto waypointname = waypointsNavigator_->getCurrentWaypointName();
// if(waypointname)
// {
// RCLCPP_INFO(
// getLogger(), "[CbNavigateNamedWaypoint] current iteration waypoints i: %ld with name '%s'",
// waypointsNavigator_->getCurrentWaypointIndex(), waypointname->c_str()); }
// else
// {
// RCLCPP_INFO(
// getLogger(), "[CbNavigateNamedWaypoint] current iteration waypoints i: %ld",
// waypointsNavigator_->getCurrentWaypointIndex()); }
auto waypointsNavigator_ = this->getComponent<CpWaypointNavigator>();
waypointsNavigator_->seekName(waypointname_);
CbNavigateNextWaypoint::onEntry();
}
void CbNavigateNamedWaypoint::onExit() { CbNavigateNextWaypoint::onExit(); }
} // namespace cl_nav2z

56
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/src/cl_nav2z/client_behaviors/cb_navigate_next_waypoint.cpp vendored
View File

@@ -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
*
******************************************************************************************************************/
#include <cl_nav2z/client_behaviors/cb_navigate_next_waypoint.hpp>
namespace cl_nav2z
{
CbNavigateNextWaypoint::CbNavigateNextWaypoint(std::optional<NavigateNextWaypointOptions> options)
{
if (options) options_ = *options;
}
CbNavigateNextWaypoint::~CbNavigateNextWaypoint() {}
void CbNavigateNextWaypoint::onEntry()
{
this->requiresComponent(waypointsNavigator_);
auto goalHandle = waypointsNavigator_->sendNextGoal(options_);
auto waypointname = waypointsNavigator_->getCurrentWaypointName();
if (waypointname)
{
RCLCPP_INFO(
getLogger(), "[CbNavigateNextWaypoint] current iteration waypoints i: %ld with name '%s'",
waypointsNavigator_->getCurrentWaypointIndex(), waypointname->c_str());
}
else
{
RCLCPP_INFO(
getLogger(), "[CbNavigateNextWaypoint] current iteration waypoints i: %ld",
waypointsNavigator_->getCurrentWaypointIndex());
}
}
void CbNavigateNextWaypoint::onExit() { waypointsNavigator_->stopWaitingResult(); }
} // namespace cl_nav2z

54
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/src/cl_nav2z/client_behaviors/cb_navigate_next_waypoint_free.cpp vendored
View File

@@ -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
*
******************************************************************************************************************/
#include <cl_nav2z/client_behaviors/cb_navigate_next_waypoint_free.hpp>
namespace cl_nav2z
{
CbNavigateNextWaypointFree::CbNavigateNextWaypointFree() {}
CbNavigateNextWaypointFree::~CbNavigateNextWaypointFree() {}
void CbNavigateNextWaypointFree::onEntry()
{
requiresComponent(this->waypointsNavigator_);
this->target_pose_ = this->waypointsNavigator_->getCurrentPose();
this->onSuccess(&CbNavigateNextWaypointFree::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 CbNavigateNextWaypointFree::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 CbNavigateNextWaypointFree::onExit() {}
} // namespace cl_nav2z

37
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/src/cl_nav2z/client_behaviors/cb_navigate_next_waypoint_until_reached.cpp vendored
View File

@@ -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
*
******************************************************************************************************************/
#include <cl_nav2z/client_behaviors/cb_navigate_next_waypoint_until_reached.hpp>
namespace cl_nav2z
{
CbNavigateNextWaypointUntilReached::CbNavigateNextWaypointUntilReached(
std::string goalWaypointName, std::optional<NavigateNextWaypointOptions> options)
: CbNavigateNextWaypoint(options), goalWaypointName_(goalWaypointName)
{
}
CbNavigateNextWaypointUntilReached::~CbNavigateNextWaypointUntilReached() {}
void CbNavigateNextWaypointUntilReached::onEntry() { CbNavigateNextWaypoint::onEntry(); }
void CbNavigateNextWaypointUntilReached::onExit() { CbNavigateNextWaypoint::onExit(); }
} // namespace cl_nav2z

51
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/src/cl_nav2z/client_behaviors/cb_pause_slam.cpp vendored
View File

@@ -1,51 +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 <cl_nav2z/client_behaviors/cb_pause_slam.hpp>
namespace cl_nav2z
{
CbPauseSlam::CbPauseSlam(std::string serviceName)
: smacc2::client_behaviors::CbServiceCall<slam_toolbox::srv::Pause>(serviceName.c_str())
{
}
void CbPauseSlam::onEntry()
{
this->requiresComponent(this->slam_);
auto currentState = slam_->getState();
if (currentState == CpSlamToolbox::SlamToolboxState::Resumed)
{
RCLCPP_INFO(
getLogger(), "[CbPauseSlam] calling pause service to toggle from resumed to paused");
this->request_ = std::make_shared<slam_toolbox::srv::Pause::Request>();
CbServiceCall<slam_toolbox::srv::Pause>::onEntry();
this->slam_->toggleState();
}
else
{
this->request_ = nullptr;
RCLCPP_INFO(
getLogger(), "[CbPauseSlam] calling skipped. The current slam state is already paused.");
}
}
} // namespace cl_nav2z

199
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/src/cl_nav2z/client_behaviors/cb_position_control_free_space.cpp vendored
View File

@@ -1,199 +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 <angles/angles.h>
#include <geometry_msgs/msg/twist.hpp>
#include <smacc2/smacc_asynchronous_client_behavior.hpp>
#include <cl_nav2z/client_behaviors/cb_position_control_free_space.hpp>
#include <cl_nav2z/components/pose/cp_pose.hpp>
namespace cl_nav2z
{
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<geometry_msgs::msg::Twist>("/cmd_vel", rclcpp::QoS(1));
cl_nav2z::Pose * 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 cl_nav2z

102
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/src/cl_nav2z/client_behaviors/cb_pure_spinning.cpp vendored
View File

@@ -1,102 +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 <angles/angles.h>
#include <cl_nav2z/client_behaviors/cb_pure_spinning.hpp>
#include <cl_nav2z/components/pose/cp_pose.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
namespace cl_nav2z
{
CbPureSpinning::CbPureSpinning(double targetYaw_rads, double max_angular_yaw_speed)
: targetYaw__rads(targetYaw_rads),
k_betta_(1.0),
max_angular_yaw_speed_(max_angular_yaw_speed),
yaw_goal_tolerance_rads_(0.1)
{
}
void CbPureSpinning::updateParameters() {}
void CbPureSpinning::onEntry()
{
auto nh = this->getNode();
cmd_vel_pub_ = nh->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", rclcpp::QoS(1));
cl_nav2z::Pose * 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(), "[" << 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();
}
this->postSuccessEvent();
}
void CbPureSpinning::onExit() {}
} // namespace cl_nav2z

51
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/src/cl_nav2z/client_behaviors/cb_resume_slam.cpp vendored
View File

@@ -1,51 +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 <cl_nav2z/client_behaviors/cb_resume_slam.hpp>
namespace cl_nav2z
{
CbResumeSlam::CbResumeSlam(std::string serviceName)
: smacc2::client_behaviors::CbServiceCall<slam_toolbox::srv::Pause>(serviceName.c_str())
{
}
void CbResumeSlam::onEntry()
{
this->requiresComponent(this->slam_);
auto currentState = slam_->getState();
if (currentState == CpSlamToolbox::SlamToolboxState::Paused)
{
RCLCPP_INFO(
getLogger(), "[CbResumeSlam] calling pause service to toggle from paused to resumed");
this->request_ = std::make_shared<slam_toolbox::srv::Pause::Request>();
smacc2::client_behaviors::CbServiceCall<slam_toolbox::srv::Pause>::onEntry();
this->slam_->toggleState();
}
else
{
this->request_ = nullptr;
RCLCPP_INFO(
getLogger(), "[CbResumeSlam] calling skipped. The current slam state is already resumed.");
}
}
} // namespace cl_nav2z

86
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/src/cl_nav2z/client_behaviors/cb_rotate.cpp vendored
View File

@@ -1,86 +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 <geometry_msgs/msg/quaternion_stamped.hpp>
#include <cl_nav2z/client_behaviors/cb_rotate.hpp>
#include <cl_nav2z/common.hpp>
#include <cl_nav2z/components/odom_tracker/cp_odom_tracker.hpp>
#include <cl_nav2z/components/pose/cp_pose.hpp>
namespace cl_nav2z
{
CbRotate::CbRotate(float rotate_degree) : rotateDegree(rotate_degree) {}
CbRotate::CbRotate(float rotate_degree, SpinningPlanner spinning_planner)
: rotateDegree(rotate_degree), spinningPlanner(spinning_planner)
{
}
void CbRotate::onEntry()
{
double angle_increment_degree = rotateDegree;
auto plannerSwitcher = this->getComponent<CpPlannerSwitcher>();
if (spinningPlanner && *spinningPlanner == SpinningPlanner::PureSpinning)
{
plannerSwitcher->setPureSpinningPlanner();
}
else
{
plannerSwitcher->setDefaultPlanners();
}
auto p = this->getComponent<cl_nav2z::Pose>();
auto referenceFrame = p->getReferenceFrame();
auto currentPoseMsg = p->toPoseMsg();
tf2::Transform currentPose;
tf2::fromMsg(currentPoseMsg, currentPose);
auto odomTracker = this->getComponent<odom_tracker::CpOdomTracker>();
nav2_msgs::action::NavigateToPose::Goal goal;
goal.pose.header.frame_id = referenceFrame;
//goal.pose.header.stamp = getNode()->now();
auto currentAngle = tf2::getYaw(currentPoseMsg.orientation);
auto targetAngle = currentAngle + angle_increment_degree * M_PI / 180.0;
goal.pose.pose.position = currentPoseMsg.position;
tf2::Quaternion q;
q.setRPY(0, 0, targetAngle);
goal.pose.pose.orientation = tf2::toMsg(q);
geometry_msgs::msg::PoseStamped stampedCurrentPoseMsg;
stampedCurrentPoseMsg.header.frame_id = referenceFrame;
stampedCurrentPoseMsg.header.stamp = getNode()->now();
stampedCurrentPoseMsg.pose = currentPoseMsg;
auto pathname = this->getCurrentState()->getName() + " - " + getName();
odomTracker->pushPath(pathname);
odomTracker->setStartPoint(stampedCurrentPoseMsg);
odomTracker->setWorkingMode(odom_tracker::WorkingMode::RECORD_PATH);
RCLCPP_INFO_STREAM(getLogger(), "current pose: " << currentPoseMsg);
RCLCPP_INFO_STREAM(getLogger(), "goal pose: " << goal.pose.pose);
this->sendGoal(goal);
}
} // namespace cl_nav2z

58
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/src/cl_nav2z/client_behaviors/cb_rotate_look_at.cpp vendored
View File

@@ -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
*
******************************************************************************************************************/
#include <cl_nav2z/cl_nav2z.hpp>
#include <cl_nav2z/client_behaviors/cb_rotate_look_at.hpp>
#include <cl_nav2z/common.hpp>
#include <cl_nav2z/components/goal_checker_switcher/cp_goal_checker_switcher.hpp>
#include <cl_nav2z/components/odom_tracker/cp_odom_tracker.hpp>
#include <cl_nav2z/components/pose/cp_pose.hpp>
#include <rclcpp/parameter_client.hpp>
namespace cl_nav2z
{
CbRotateLookAt::CbRotateLookAt() {}
CbRotateLookAt::CbRotateLookAt(const geometry_msgs::msg::PoseStamped & lookAtPose)
: lookAtPose_(lookAtPose)
{
}
void CbRotateLookAt::onEntry()
{
cl_nav2z::Pose * pose;
this->requiresComponent(pose);
pose->waitTransformUpdate(rclcpp::Rate(20));
auto position = pose->toPoseMsg().position;
if (lookAtPose_)
{
auto targetPosition = lookAtPose_->pose.position;
double yaw_degrees =
atan2(targetPosition.y - position.y, targetPosition.x - position.x) * 180.0 / M_PI;
this->absoluteGoalAngleDegree = yaw_degrees;
}
CbAbsoluteRotate::onEntry();
}
} // namespace cl_nav2z

85
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/src/cl_nav2z/client_behaviors/cb_save_slam_map.cpp vendored
View File

@@ -1,85 +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 <angles/angles.h>
#include <chrono>
#include <cl_nav2z/client_behaviors/cb_save_slam_map.hpp>
#include <cl_nav2z/components/pose/cp_pose.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <memory>
#include <nav2_msgs/srv/save_map.hpp>
#include <slam_toolbox/srv/save_map.hpp>
#include <smacc2/client_behaviors/cb_call_service.hpp>
#include <smacc2/smacc_asynchronous_client_behavior.hpp>
#include <std_msgs/msg/string.hpp>
namespace cl_nav2z
{
using namespace std::chrono_literals;
CbSaveSlamMap::CbSaveSlamMap() : CbServiceCall("/map_saver/save_map", getRequest())
{
// : CbServiceCall("/slam_toolbox/save_map",
// getRequest()) {
// map_name.data = "saved_map";
// auto request = getRequest(map_name);
// RCLCPP_INFO_STREAM(getLogger(), "Save Slam Map builded");
}
// void onEntry() override {}
void CbSaveSlamMap::onExit() {}
std::shared_ptr<nav2_msgs::srv::SaveMap::Request> CbSaveSlamMap::getRequest(
/*slam_toolbox::srv::SaveMap_Request_<std::allocator<void> >::_name_type saved_map_name*/)
{
nav2_msgs::srv::SaveMap_Request map_save;
std_msgs::msg::String map_name;
// // map_name.data = "saved_map";
// map_save.map_topic = "map";
// map_save.map_url = "${workspacesFolder}/maps/saved_map";
// map_save.image_format = "png";
// map_save.occupied_thresh = 0.65;
// map_save.free_thresh = 0.25;
// map_save.map_mode = "trinary";
// // auto request = std::make_shared<slam_toolbox::srv::SaveMap::Request>();
// // // request->name = saved_map_name;
// // request->name = map_name;
// // return request;
// auto request = std::make_shared<nav2_msgs::srv::SaveMap::Request>(map_save);
auto request = std::make_shared<nav2_msgs::srv::SaveMap::Request>();
request->map_topic = "map";
request->map_url = "/tmp/saved_map";
request->image_format = "png";
request->occupied_thresh = 0.65;
request->free_thresh = 0.25;
request->map_mode = "trinary";
return request;
}
} // namespace cl_nav2z
// slam_toolbox::srv::SaveMap_Request_<std::allocator<void> >::_name_type
// std_msgs::msg::String_<std::allocator<void> >

50
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/src/cl_nav2z/client_behaviors/cb_seek_waypoint.cpp vendored
View File

@@ -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
*
******************************************************************************************************************/
#include <cl_nav2z/client_behaviors/cb_seek_waypoint.hpp>
namespace cl_nav2z
{
CbSeekWaypoint::CbSeekWaypoint(std::string seekWaypointName)
: count_(std::nullopt), seekWaypointName_(seekWaypointName)
{
}
CbSeekWaypoint::~CbSeekWaypoint() {}
void CbSeekWaypoint::onEntry()
{
this->requiresComponent(waypointsNavigator_);
if (count_)
{
waypointsNavigator_->forward(*count_);
count_ = std::nullopt;
}
else if (seekWaypointName_)
{
waypointsNavigator_->seekName(*seekWaypointName_);
seekWaypointName_ = std::nullopt;
}
}
void CbSeekWaypoint::onExit() {}
} // namespace cl_nav2z

136
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/src/cl_nav2z/client_behaviors/cb_spiral_motion.cpp vendored
View File

@@ -1,136 +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 <angles/angles.h>
#include <cl_nav2z/client_behaviors/cb_spiral_motion.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <optional>
#include <smacc2/smacc_asynchronous_client_behavior.hpp>
namespace cl_nav2z
{
CbSpiralMotion::CbSpiralMotion(std::optional<CbSpiralMotionOptions> options)
{
if (options)
{
options_ = *options;
}
else
{
// we use default options
options_ = CbSpiralMotionOptions();
}
}
void CbSpiralMotion::onEntry()
{
/*
struct CbSpiralMotionOptions
{
std::optional<float> linearVelocity = 0.0f;
std::optional<float> maxLinearVelocity = 1.0f;
std::optional<float> initialAngularVelocity = 1.0f;
std::optional<rclcpp::Duration> spiralMotionDuration = rclcpp::Duration::from_seconds(120);
std::optional <float> finalRadius=10.0f;//meters
};
*/
auto linearVelocity = *(options_.linearVelocity);
auto maxLinearVelocity = *(options_.maxLinearVelocity);
auto initialAngularVelocity = *(options_.initialAngularVelocity);
auto spiralMotionDuration = *(options_.spiralMotionDuration);
auto finalRadius = *(options_.finalRadius);
float rate = 20.0f;
rclcpp::Rate r(rate);
cmdVelPub_ = getNode()->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", rclcpp::QoS(1));
rclcpp::Duration linearRamp = rclcpp::Duration::from_seconds(spiralMotionDuration.seconds());
float linearAceleration = (maxLinearVelocity - linearVelocity) / linearRamp.seconds();
float dt = 1.0f / rate;
// we know final radious and the constant linear velocity
float finalAngularVelocity = maxLinearVelocity / finalRadius;
float angularAcceleration =
(initialAngularVelocity - finalAngularVelocity) / spiralMotionDuration.seconds();
geometry_msgs::msg::Twist cmd_vel;
cmd_vel.linear.x = linearVelocity;
cmd_vel.angular.z = initialAngularVelocity;
auto start_time = getNode()->now();
RCLCPP_INFO_STREAM(
getLogger(), "[CbSpiralMotion]: initialAngularVelocity: "
<< initialAngularVelocity << ", finalAngularVelocity: " << finalAngularVelocity
<< ", angularAcceleration: " << angularAcceleration);
RCLCPP_INFO_STREAM(
getLogger(), "[CbSpiralMotion]: linearAceleration: "
<< linearAceleration << ", maxLinearVelocity: " << maxLinearVelocity);
bool end_condition = false;
while (!end_condition)
{
auto current_time = getNode()->now() - start_time;
cmd_vel.linear.x += linearAceleration * dt;
if (cmd_vel.linear.x > maxLinearVelocity)
{
cmd_vel.linear.x = maxLinearVelocity;
}
float signVal = (cmd_vel.angular.z >= 0.0f) ? 1.0f : -1.0f;
// cmd_vel.angular.z -= signVal * angularAcceleration * dt;
float ellapsedTimeFactor = current_time.seconds() / spiralMotionDuration.seconds();
cmd_vel.angular.z = initialAngularVelocity * (1.0f - ellapsedTimeFactor) +
finalAngularVelocity * ellapsedTimeFactor;
RCLCPP_INFO(
getLogger(), "[CbSpiralMotion] cmd_vel.linear.x = %f, cmd_vel.angular.z = %f",
cmd_vel.linear.x, cmd_vel.angular.z);
cmdVelPub_->publish(cmd_vel);
r.sleep();
auto now = getNode()->now();
rclcpp::Duration ellapsed = now - start_time;
RCLCPP_INFO_STREAM(
getLogger(), "[CbSpiralMotion] ellapsed time: " << ellapsed.seconds() << ", total duration: "
<< spiralMotionDuration.seconds());
if (ellapsed > spiralMotionDuration)
{
RCLCPP_INFO_STREAM(getLogger(), "[CbSpiralMotion] spiralMotionDuration reached");
end_condition = true;
}
}
// asynchronous client behaviors usually post a success event when they are done
// that is used in states to transition to the next state
this->postSuccessEvent();
}
void CbSpiralMotion::onExit() {}
} // namespace cl_nav2z

52
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/src/cl_nav2z/client_behaviors/cb_stop_navigation.cpp vendored
View File

@@ -1,52 +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 <cl_nav2z/cl_nav2z.hpp>
#include <cl_nav2z/client_behaviors/cb_abort_navigation.hpp>
#include <cl_nav2z/common.hpp>
#include <cl_nav2z/components/goal_checker_switcher/cp_goal_checker_switcher.hpp>
#include <cl_nav2z/components/odom_tracker/cp_odom_tracker.hpp>
#include <cl_nav2z/components/pose/cp_pose.hpp>
#include <rclcpp/parameter_client.hpp>
namespace cl_nav2z
{
CbStopNavigation::CbStopNavigation() {}
void CbStopNavigation::onEntry()
{
// this->sendGoal(goal);
// this->cancelGoal();
cl_nav2z::Pose * poseComponent;
this->requiresComponent(poseComponent);
this->setGoal(poseComponent->toPoseMsg());
CbNavigateGlobalPosition::onEntry();
// this->sendGoal(goal);
}
void CbStopNavigation::onExit() {}
} // namespace cl_nav2z

49
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/src/cl_nav2z/client_behaviors/cb_track_path_odometry.cpp vendored
View File

@@ -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 <cl_nav2z/client_behaviors/cb_track_path_odometry.hpp>
#include <cl_nav2z/components/odom_tracker/cp_odom_tracker.hpp>
#include <cl_nav2z/components/pose/cp_pose.hpp>
namespace cl_nav2z
{
CbTrackPathOdometry::CbTrackPathOdometry() {}
void CbTrackPathOdometry::onEntry()
{
RCLCPP_INFO(this->getLogger(), "Pose tracker freeze reference frame");
cl_nav2z::Pose * poseComponent;
requiresComponent(poseComponent);
poseComponent->freezeReferenceFrame();
// poseComponent->setReferenceFrame("odom");
RCLCPP_INFO(this->getLogger(), "Odom tracker clear path");
cl_nav2z::odom_tracker::CpOdomTracker * odomTracker;
this->requiresComponent(odomTracker);
// odomTracker->setOdomFrame("odom");
odomTracker->clearPath();
}
void CbTrackPathOdometry::onExit() {}
} // namespace cl_nav2z

49
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/src/cl_nav2z/client_behaviors/cb_track_path_slam.cpp vendored
View File

@@ -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 <cl_nav2z/client_behaviors/cb_track_path_slam.hpp>
#include <cl_nav2z/components/odom_tracker/cp_odom_tracker.hpp>
#include <cl_nav2z/components/pose/cp_pose.hpp>
namespace cl_nav2z
{
CbTrackPathSLAM::CbTrackPathSLAM() {}
void CbTrackPathSLAM::onEntry()
{
RCLCPP_INFO(this->getLogger(), "Pose tracker freeze reference frame");
cl_nav2z::Pose * poseComponent;
requiresComponent(poseComponent);
poseComponent->unfreezeReferenceFrame();
// poseComponent->setReferenceFrame("odom");
RCLCPP_INFO(this->getLogger(), "Odom tracker clear path");
cl_nav2z::odom_tracker::CpOdomTracker * odomTracker;
this->requiresComponent(odomTracker);
// odomTracker->setOdomFrame("odom");
odomTracker->clearPath();
}
void CbTrackPathSLAM::onExit() {}
} // namespace cl_nav2z

131
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/src/cl_nav2z/client_behaviors/cb_undo_path_backwards.cpp vendored
View File

@@ -1,131 +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 <cl_nav2z/client_behaviors/cb_undo_path_backwards.hpp>
#include <cl_nav2z/common.hpp>
#include <cl_nav2z/components/goal_checker_switcher/cp_goal_checker_switcher.hpp>
#include <cl_nav2z/components/planner_switcher/cp_planner_switcher.hpp>
namespace cl_nav2z
{
using ::cl_nav2z::odom_tracker::CpOdomTracker;
using ::cl_nav2z::odom_tracker::WorkingMode;
using namespace std::chrono_literals;
CbUndoPathBackwards::CbUndoPathBackwards(std::optional<CbUndoPathBackwardsOptions> options)
{
options_ = options;
}
void CbUndoPathBackwards::onEntry()
{
listener = std::make_shared<tf2_ros::Buffer>(this->getNode()->get_clock());
odomTracker = this->getComponent<CpOdomTracker>();
odomTracker->logStateString(false);
auto plannerSwitcher = this->getComponent<CpPlannerSwitcher>();
nav_msgs::msg::Path forwardpath = odomTracker->getPath();
// RCLCPP_INFO_STREAM(getLogger(),"[UndoPathBackward] Current path backwards: " << forwardpath);
odomTracker->setWorkingMode(WorkingMode::CLEAR_PATH);
nav2_msgs::action::NavigateToPose::Goal goal;
auto goalCheckerSwitcher = this->getComponent<CpGoalCheckerSwitcher>();
// this line is used to flush/reset backward planner in the case it were already there
// plannerSwitcher->setDefaultPlanners();
if (forwardpath.poses.size() > 0)
{
goal.pose = forwardpath.poses.front();
//goal.pose.header.stamp = getNode()->now();
goal.pose.header.stamp = rclcpp::Time(0);
if (options_ && options_->undoControllerName_)
{
plannerSwitcher->setUndoPathBackwardPlanner(false);
RCLCPP_INFO_STREAM(
getLogger(),
"[" << getName() << "] Undoing path with controller: " << *options_->undoControllerName_);
plannerSwitcher->setDesiredController(*options_->undoControllerName_);
plannerSwitcher->commitPublish();
}
else
{
plannerSwitcher->setUndoPathBackwardPlanner();
}
// WARNING: There might be some race condition with the remote undo global planner/controller were the global path was not
// received yet, thee goal switcher
// TODO: waiting notification from global planner that it is loaded
RCLCPP_ERROR_STREAM(
getLogger(), "[" << getName()
<< "] Waiting for 5 seconds to get planer/controller and wait goal checker "
"ready");
// rclcpp::sleep_for(5s);
RCLCPP_ERROR_STREAM(getLogger(), "[" << getName() << "] activating undo navigation planner");
if (options_ && options_->goalCheckerId_)
{
goalCheckerSwitcher->setGoalCheckerId(*options_->goalCheckerId_);
}
else
{
goalCheckerSwitcher->setGoalCheckerId("undo_path_backwards_goal_checker");
}
this->sendGoal(goal);
}
}
void CbUndoPathBackwards::onExit()
{
RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] Exiting: undo navigation ");
if (this->navigationResult_ == rclcpp_action::ResultCode::SUCCEEDED)
{
RCLCPP_INFO_STREAM(
getLogger(), getName() << " - [CbUndoPathBackwards] Exiting: undo navigation successful, "
"popping odom tracker path");
odomTracker = this->getComponent<CpOdomTracker>();
odomTracker->popPath();
odomTracker->logStateString(false);
}
else
{
RCLCPP_INFO_STREAM(
getLogger(), getName() << " - [CbUndoPathBackwards] Exiting: undo navigation abort, avoiding "
"popping current path");
odomTracker = this->getComponent<CpOdomTracker>();
odomTracker->logStateString(false);
// navigation interrupted or aborted. The path may be not totally undone.
// We keep the odom tracker in its current state, probably in the middle of the undoing process.
// Could you try to repeat the behavior?
}
}
} // namespace cl_nav2z

133
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/src/cl_nav2z/client_behaviors/cb_wait_nav2_nodes.cpp vendored
View File

@@ -1,133 +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 <cl_nav2z/client_behaviors/cb_wait_nav2_nodes.hpp>
#include <cl_nav2z/common.hpp>
namespace cl_nav2z
{
CbWaitNav2Nodes::CbWaitNav2Nodes(std::vector<Nav2Nodes> waitNodes)
: CbWaitNav2Nodes("/bond", waitNodes)
{
}
CbWaitNav2Nodes::CbWaitNav2Nodes(std::string topic, std::vector<Nav2Nodes> waitNodes)
: topicname_(topic), waitNodes_(waitNodes)
{
}
void CbWaitNav2Nodes::onMessageReceived(const bond::msg::Status & msg)
{
auto value = fromString(msg.id);
bool updated = false;
// RCLCPP_INFO(getLogger(), "[CbWaitNav2Nodes] received '%s'", msg.id.c_str());
if (receivedAliveMsg_.count(value) && !receivedAliveMsg_[value])
{
RCLCPP_INFO(getLogger(), "[CbWaitNav2Nodes] '%s' alive received", msg.id.c_str());
receivedAliveMsg_[value] = true;
updated = true;
}
if (updated)
{
bool success = true;
std::stringstream ss;
for (auto & pair : receivedAliveMsg_)
{
if (!pair.second)
{
success = false;
}
ss << "- " << toString(pair.first) << ": " << (pair.second ? "ALIVE" : "WAITING")
<< std::endl;
}
RCLCPP_INFO_STREAM(
getLogger(), "[CbWaitNav2Nodes] waiting nodes status:" << std::endl
<< ss.str().c_str());
if (success)
{
RCLCPP_INFO(getLogger(), "[CbWaitNav2Nodes] success event");
this->postSuccessEvent();
sub_ = nullptr;
}
else
{
RCLCPP_INFO(getLogger(), "[CbWaitNav2Nodes] still missing nodes");
}
}
}
void CbWaitNav2Nodes::onEntry()
{
std::stringstream ss;
for (auto v : waitNodes_)
{
receivedAliveMsg_[v] = false;
ss << "[CbWaitNav2Nodes] - " << toString(v) << ": waiting" << std::endl;
}
RCLCPP_INFO(getLogger(), ss.str().c_str());
//rclcpp::SensorDataQoS qos;
rclcpp::SubscriptionOptions sub_option;
sub_ = getNode()->create_subscription<bond::msg::Status>(
topicname_, 20, std::bind(&CbWaitNav2Nodes::onMessageReceived, this, std::placeholders::_1),
sub_option);
}
std::string toString(Nav2Nodes value)
{
switch (value)
{
case Nav2Nodes::PlannerServer:
return "planner_server";
case Nav2Nodes::ControllerServer:
return "controller_server";
case Nav2Nodes::RecoveriesServer:
return "behavior_server";
case Nav2Nodes::BtNavigator:
return "bt_navigator";
case Nav2Nodes::MapServer:
return "map_server";
default:
return "";
}
}
Nav2Nodes fromString(std::string id)
{
if (id == "planner_server")
return Nav2Nodes::PlannerServer;
else if (id == "controller_server")
return Nav2Nodes::ControllerServer;
else if (id == "behavior_server")
return Nav2Nodes::RecoveriesServer;
else if (id == "bt_navigator")
return Nav2Nodes::BtNavigator;
else if (id == "map_server")
return Nav2Nodes::MapServer;
else
return Nav2Nodes::None;
}
} // namespace cl_nav2z

53
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/src/cl_nav2z/client_behaviors/cb_wait_pose.cpp vendored
View File

@@ -1,53 +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 <cl_nav2z/client_behaviors/cb_wait_pose.hpp>
#include <cl_nav2z/components/pose/cp_pose.hpp>
#include <cl_nav2z/common.hpp>
#include <rclcpp/parameter_client.hpp>
namespace cl_nav2z
{
CbWaitPose::CbWaitPose() {}
CbWaitPose::~CbWaitPose() {}
void CbWaitPose::onEntry()
{
cl_nav2z::Pose * pose = nullptr;
this->requiresComponent(pose);
try
{
pose->waitTransformUpdate(rclcpp::Rate(20));
auto posemsg = pose->toPoseMsg();
RCLCPP_INFO_STREAM(getLogger(), "[CbWaitPose] pose arrived: " << std::endl << posemsg);
}
catch (std::exception & ex)
{
RCLCPP_INFO(getLogger(), "[CbWaitPose] error getting the robot pose");
this->postFailureEvent();
return;
}
RCLCPP_INFO(getLogger(), "[CbWaitPose] pose received");
this->postSuccessEvent();
}
} // namespace cl_nav2z

63
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/src/cl_nav2z/client_behaviors/cb_wait_transform.cpp vendored
View File

@@ -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
*
******************************************************************************************************************/
#include <cl_nav2z/client_behaviors/cb_wait_transform.hpp>
namespace cl_nav2z
{
CbWaitTransform::CbWaitTransform(
std::string targetFrame, std::string referenceFrame, rclcpp::Duration timeout)
: targetFrame_(targetFrame), referenceFrame_(referenceFrame), timeout_(timeout)
{
}
CbWaitTransform::~CbWaitTransform() {}
void CbWaitTransform::onEntry()
{
RCLCPP_INFO(
getLogger(), "[CbWaitTransform] ref %s -> target %s", referenceFrame_.c_str(),
targetFrame_.c_str());
tfBuffer_ = std::make_shared<tf2_ros::Buffer>(getNode()->get_clock());
tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
tf2::Stamped<tf2::Transform> transform;
try
{
auto transformstamped =
tfBuffer_->lookupTransform(targetFrame_, referenceFrame_, getNode()->now(), timeout_);
tf2::fromMsg(transformstamped, transform);
result_ = transform;
RCLCPP_INFO(
getLogger(), "[CbWaitTransform] Success wait transform ref %s -> target %s",
referenceFrame_.c_str(), targetFrame_.c_str());
this->postSuccessEvent();
}
catch (tf2::TransformException & ex)
{
RCLCPP_ERROR_STREAM(
getLogger(), "[CbWaitTransform] Failure waiting transform ( ref "
<< targetFrame_ << "/ target " << referenceFrame_ << " - " << ex.what());
this->postFailureEvent();
}
}
} // namespace cl_nav2z

64
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/src/cl_nav2z/common.cpp vendored
View File

@@ -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
*
******************************************************************************************************************/
#include <cl_nav2z/common.hpp>
#include <tf2/utils.h>
#include <rclcpp/rclcpp.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
std::ostream & operator<<(std::ostream & out, const geometry_msgs::msg::Quaternion & msg)
{
out << " Orientation [" << msg.x << " , " << msg.y << " , " << msg.z << ", " << msg.w
<< "] , yaw: " << tf2::getYaw(msg);
return out;
}
std::ostream & operator<<(std::ostream & out, const geometry_msgs::msg::Point & msg)
{
out << "[" << msg.x << " , " << msg.y << " , " << msg.z << "]";
return out;
}
std::ostream & operator<<(std::ostream & out, const geometry_msgs::msg::Pose & msg)
{
out << " p " << msg.position;
out << " q [" << msg.orientation.x << " , " << msg.orientation.y << " , " << msg.orientation.z
<< ", " << msg.orientation.w << "]";
return out;
}
std::ostream & operator<<(std::ostream & out, const geometry_msgs::msg::PoseStamped & msg)
{
out << msg.pose;
return out;
}
std::ostream & operator<<(std::ostream & out, const nav2_msgs::action::NavigateToPose::Goal & msg)
{
out << msg.pose;
return out;
}
std::ostream & operator<<(std::ostream & out, const builtin_interfaces::msg::Time & msg)
{
out << "seconds: " << rclcpp::Time(msg).seconds();
return out;
}

43
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/src/cl_nav2z/components/amcl/cp_amcl.cpp vendored
View File

@@ -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
*
******************************************************************************************************************/
#include <cl_nav2z/components/amcl/cp_amcl.hpp>
#include <string>
namespace cl_nav2z
{
CpAmcl::CpAmcl() {}
CpAmcl::~CpAmcl() {}
std::string CpAmcl::getName() const { return "AMCL"; }
void CpAmcl::onInitialize()
{
initalPosePub_ = getNode()->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
"initialpose", rclcpp::QoS(10));
}
void CpAmcl::setInitialPose(const geometry_msgs::msg::PoseWithCovarianceStamped & initialpose)
{
initalPosePub_->publish(initialpose);
}
} // namespace cl_nav2z

162
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/src/cl_nav2z/components/costmap_switch/cp_costmap_switch.cpp vendored
View File

@@ -1,162 +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 <cl_nav2z/components/costmap_switch/cp_costmap_switch.hpp>
namespace cl_nav2z
{
std::array<std::string, 4> CpCostmapSwitch::layerNames = {
"global_costmap/obstacles_layer",
"local_costmap/obstacles_layer"
"global_costmap/inflater_layer",
"local_costmap/inflater_layer"};
void CpCostmapSwitch::registerProxyFromDynamicReconfigureServer(
std::string costmapName, std::string enablePropertyName)
{
RCLCPP_INFO(getLogger(), "[CpCostmapSwitch] registering costmap type: %s", costmapName.c_str());
auto proxy = std::make_shared<CpCostmapProxy>(
this->nav2zClient_->getName() + "/" + costmapName, enablePropertyName, getNode());
costmapProxies[costmapName] = proxy;
}
CpCostmapSwitch::CpCostmapSwitch() {}
void CpCostmapSwitch::onInitialize()
{
this->nav2zClient_ = dynamic_cast<cl_nav2z::ClNav2Z *>(owner_);
if (this->nav2zClient_ == nullptr)
{
RCLCPP_ERROR(getLogger(), "the owner of the CpCostmapSwitch must be a ClNav2Z");
}
registerProxyFromDynamicReconfigureServer(
getStandardCostmapName(StandardLayers::GLOBAL_OBSTACLES_LAYER));
registerProxyFromDynamicReconfigureServer(
getStandardCostmapName(StandardLayers::LOCAL_OBSTACLES_LAYER));
registerProxyFromDynamicReconfigureServer(
getStandardCostmapName(StandardLayers::GLOBAL_INFLATED_LAYER));
registerProxyFromDynamicReconfigureServer(
getStandardCostmapName(StandardLayers::LOCAL_INFLATED_LAYER));
}
std::string CpCostmapSwitch::getStandardCostmapName(StandardLayers layertype)
{
return CpCostmapSwitch::layerNames[(int)layertype];
}
bool CpCostmapSwitch::exists(std::string layerName)
{
if (!this->costmapProxies.count(layerName))
{
return false;
}
return true;
}
void CpCostmapSwitch::enable(std::string layerName)
{
RCLCPP_INFO(getLogger(), "[CpCostmapSwitch] enabling %s", layerName.c_str());
if (!exists(layerName))
{
RCLCPP_ERROR(getLogger(), "[CpCostmapSwitch] costmap %s does not exist", layerName.c_str());
return;
}
else
{
RCLCPP_INFO(
getLogger(), "[CpCostmapSwitch] costmap %s found. Calling dynamic reconfigure server.",
layerName.c_str());
costmapProxies[layerName]->setCostmapEnabled(true);
}
}
void CpCostmapSwitch::enable(StandardLayers layerType)
{
this->enable(getStandardCostmapName(layerType));
}
void CpCostmapSwitch::disable(std::string layerName)
{
RCLCPP_INFO(getLogger(), "[CpCostmapSwitch] disabling %s", layerName.c_str());
if (!exists(layerName))
{
RCLCPP_ERROR(getLogger(), "[CpCostmapSwitch] costmap %s does not exist", layerName.c_str());
return;
}
else
{
RCLCPP_INFO(
getLogger(), "[CpCostmapSwitch] costmap %s found. Calling dynamic reconfigure server.",
layerName.c_str());
costmapProxies[layerName]->setCostmapEnabled(false);
}
}
void CpCostmapSwitch::disable(StandardLayers layerType)
{
this->disable(getStandardCostmapName(layerType));
}
//-------------------------------------------------------------------------
CpCostmapProxy::CpCostmapProxy(
std::string /*costmap_name*/, std::string /*enablePropertyName*/, rclcpp::Node::SharedPtr nh)
: nh_(nh)
{
// this->costmapName_ = costmap_name + "/set_parameters";
// dynamic_reconfigure::BoolParameter enableField;
// enableField.name = "enabled";
// enableField.value = true;
// enableReq.bools.push_back(enableField);
// enableField.value = false;
// disableReq.bools.push_back(enableField);
RCLCPP_ERROR(nh->get_logger(), "costmap switch not implemented %s", costmapName_.c_str());
}
void CpCostmapProxy::setCostmapEnabled(bool /*value*/)
{
// dynamic_reconfigure::ReconfigureRequest srv_req;
// dynamic_reconfigure::ReconfigureResponse srv_resp;
// if (value)
// srv_req.config = enableReq;
// else
// srv_req.config = disableReq;
// if (ros::service::exists(costmapName_, true))
// {
// RCLCPP_INFO(getLogger(),"sending dynamic reconfigure request: %s", costmapName_.c_str());
// ros::service::call(costmapName_, srv_req, srv_resp);
// }
// else
// {
// RCLCPP_WARN(getLogger(),"could not call dynamic reconfigure server. It does not exist: %s", costmapName_.c_str());
// }
RCLCPP_ERROR(nh_->get_logger(), "costmap switch not implemented %s", costmapName_.c_str());
}
// void CpCostmapProxy::dynreconfCallback(const dynamic_reconfigure::Config::ConstPtr &configuration_update)
// {
// // auto gp = std::find_if(configuration_update->strs.begin(), configuration_update->strs.begin(),
// // [&](const dynamic_reconfigure::StrParameter &p) { return p.name == "base_global_planner" && p.value == desired_global_planner_; });
// }
} // namespace cl_nav2z

75
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/src/cl_nav2z/components/goal_checker_switcher/cp_goal_checker_switcher.cpp vendored
View File

@@ -1,75 +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 <cl_nav2z/components/goal_checker_switcher/cp_goal_checker_switcher.hpp>
namespace cl_nav2z
{
CpGoalCheckerSwitcher::CpGoalCheckerSwitcher(
std::string goal_checker_selector_topic, std::string default_goal_checker_name)
: goal_checker_selector_topic_(goal_checker_selector_topic),
default_goal_checker_name_(default_goal_checker_name)
{
}
CpGoalCheckerSwitcher::~CpGoalCheckerSwitcher() {}
void CpGoalCheckerSwitcher::onInitialize()
{
rclcpp::QoS qos(rclcpp::KeepLast(1));
qos.transient_local().reliable();
this->goal_checker_selector_pub_ =
getNode()->create_publisher<std_msgs::msg::String>(goal_checker_selector_topic_, qos);
}
void CpGoalCheckerSwitcher::setDefaultGoalChecker()
{
setGoalCheckerId(default_goal_checker_name_); // default id in navigation2 stack
}
void CpGoalCheckerSwitcher::setGoalCheckerId(std::string goalcheckerid)
{
RCLCPP_INFO_STREAM(
getLogger(), "[CpGoalCheckerSwitcher] Setting goal checker: " << goalcheckerid);
// controller_server_node_->wait_for_service();
// std::vector<rclcpp::Parameter> params{ rclcpp::Parameter("current_goal_checker", goalcheckerid) };
// auto futureResults = controller_server_node_->set_parameters(params);
std_msgs::msg::String msg;
msg.data = goalcheckerid;
this->goal_checker_selector_pub_->publish(msg);
// int i = 0;
// for (auto& res : futureResults.get())
// {
// RCLCPP_INFO_STREAM(getLogger(), "[CpGoalCheckerSwitcher] parameter result: "
// << params[i].get_name() << "=" << params[i].as_string()
// << ". Result: " << res.successful);
// i++;
// if (!res.successful)
// RCLCPP_ERROR_STREAM(this->getLogger(), "[CpGoalCheckerSwitcher] goal checker could not properly
// switch "
// "the goal checker of the controller_server");
// }
}
} // namespace cl_nav2z

658
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/src/cl_nav2z/components/odom_tracker/cp_odom_tracker.cpp vendored
View File

@@ -1,658 +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 <cl_nav2z/common.hpp>
#include <angles/angles.h>
#include <tf2/utils.h>
#include <cl_nav2z/components/odom_tracker/cp_odom_tracker.hpp>
#include <smacc2/component.hpp>
#include <smacc2/impl/smacc_component_impl.hpp>
#include <boost/range/adaptor/reversed.hpp>
namespace cl_nav2z
{
namespace odom_tracker
{
CpOdomTracker::CpOdomTracker(
std::string odomTopicName, std::string odomFrame, OdomTrackerStrategy strategy)
: strategy_(strategy)
{
workingMode_ = WorkingMode::RECORD_PATH;
publishMessages = true;
odomFrame_ = odomFrame;
odomTopicName_ = odomTopicName;
}
template <typename T>
void parameterDeclareAndtryGetOrSet(
rclcpp::Node::SharedPtr & node, std::string param_name, T & value)
{
if (!node->get_parameter(param_name, value))
{
RCLCPP_INFO_STREAM(
node->get_logger(), "[CpOdomTracker] autoset " << param_name << ": " << value);
node->declare_parameter(param_name, value);
}
else
{
RCLCPP_INFO_STREAM(node->get_logger(), "[CpOdomTracker] " << param_name << ": " << value);
}
}
void CpOdomTracker::onInitialize()
{
// default values
recordPointDistanceThreshold_ = 0.005; // 5 mm
recordAngularDistanceThreshold_ = 0.1; // radians
clearPointDistanceThreshold_ = 0.05; // 5 cm
clearAngularDistanceThreshold_ = 0.1; // radi
auto nh = getNode();
RCLCPP_WARN(getLogger(), "[CpOdomTracker] Initializing Odometry Tracker");
parameterDeclareAndtryGetOrSet(nh, "odom_frame", this->odomFrame_);
parameterDeclareAndtryGetOrSet(
nh, "record_point_distance_threshold", recordPointDistanceThreshold_);
parameterDeclareAndtryGetOrSet(
nh, "record_angular_distance_threshold", recordAngularDistanceThreshold_);
parameterDeclareAndtryGetOrSet(
nh, "clear_point_distance_threshold", clearPointDistanceThreshold_);
parameterDeclareAndtryGetOrSet(
nh, "clear_angular_distance_threshold", clearAngularDistanceThreshold_);
param_callback_handle_ = getNode()->add_on_set_parameters_callback(
std::bind(&CpOdomTracker::parametersCallback, this, std::placeholders::_1));
robotBasePathPub_ =
nh->create_publisher<nav_msgs::msg::Path>("odom_tracker_path", rclcpp::QoS(1));
robotBasePathStackedPub_ =
nh->create_publisher<nav_msgs::msg::Path>("odom_tracker_stacked_path", rclcpp::QoS(1));
if (this->strategy_ == OdomTrackerStrategy::ODOMETRY_SUBSCRIBER)
{
RCLCPP_INFO_STREAM(
nh->get_logger(), "[CpOdomTracker] subscribing to odom topic: " << odomTopicName_);
rclcpp::SensorDataQoS qos;
odomSub_ = nh->create_subscription<nav_msgs::msg::Odometry>(
odomTopicName_, qos,
std::bind(&CpOdomTracker::odomMessageCallback, this, std::placeholders::_1));
}
else if (this->strategy_ == OdomTrackerStrategy::POSE_COMPONENT)
{
this->requiresComponent(robotPose_);
robotPoseTimer_ = nh->create_wall_timer(
std::chrono::milliseconds(100), std::bind(&CpOdomTracker::update, this));
}
else
{
RCLCPP_ERROR_STREAM(
nh->get_logger(), "[CpOdomTracker] unknown strategy: " << (int)this->strategy_);
}
}
/**
******************************************************************************************************************
* parametersCallback()
******************************************************************************************************************
*/
rcl_interfaces::msg::SetParametersResult CpOdomTracker::parametersCallback(
const std::vector<rclcpp::Parameter> & parameters)
{
std::lock_guard<std::mutex> lock(m_mutex_);
RCLCPP_INFO(getLogger(), "odom_tracker m_mutex acquire");
for (const auto & param : parameters)
{
if (param.get_name() == "record_point_distance_threshold")
{
recordPointDistanceThreshold_ = param.as_double();
}
else if (param.get_name() == "record_angular_distance_threshold")
{
recordAngularDistanceThreshold_ = param.as_double();
}
else if (param.get_name() == "clear_point_distance_threshold")
{
clearPointDistanceThreshold_ = param.as_double();
}
else if (param.get_name() == "clear_angular_distance_threshold")
{
clearAngularDistanceThreshold_ = param.as_double();
}
else if (param.get_name() == "odom_frame")
{
odomFrame_ = param.as_string();
}
}
rcl_interfaces::msg::SetParametersResult result;
result.successful = true;
result.reason = "success";
return result;
}
/**
******************************************************************************************************************
* setWorkingMode()
******************************************************************************************************************
*/
void CpOdomTracker::setWorkingMode(WorkingMode workingMode)
{
std::lock_guard<std::mutex> lock(m_mutex_);
switch (workingMode)
{
case WorkingMode::RECORD_PATH:
RCLCPP_INFO_STREAM(
getLogger(),
"[CpOdomTracker] setting working mode to RECORD - record_point_distance_threshold: "
<< recordPointDistanceThreshold_
<< ", record_angular_distance_threshold: " << recordAngularDistanceThreshold_);
break;
case WorkingMode::CLEAR_PATH:
RCLCPP_INFO_STREAM(
getLogger(),
"[CpOdomTracker] setting working mode to CLEAR - clear_point_distance_threshold: "
<< clearPointDistanceThreshold_
<< ", clear_angular_distance_threshold: " << clearAngularDistanceThreshold_);
break;
case WorkingMode::IDLE:
RCLCPP_INFO_STREAM(getLogger(), "[CpOdomTracker] setting working mode to IDLE");
break;
default:
RCLCPP_INFO_STREAM(getLogger(), "[CpOdomTracker] setting working mode to <UNKNOWN>");
}
if (this->odomSub_ != nullptr && this->odomSub_->get_publisher_count() == 0)
{
RCLCPP_ERROR_STREAM(
getLogger(),
"[CpOdomTracker] Odom tracker cannot record the path because there is no publisher to the "
"odometry topic");
}
workingMode_ = workingMode;
// RCLCPP_INFO(getLogger(),"odom_tracker m_mutex release");
}
/**
******************************************************************************************************************
* setPublishMessages()
******************************************************************************************************************
*/
void CpOdomTracker::setPublishMessages(bool value)
{
// RCLCPP_INFO(getLogger(),"odom_tracker m_mutex acquire");
std::lock_guard<std::mutex> lock(m_mutex_);
publishMessages = value;
// RCLCPP_INFO(getLogger(),"odom_tracker m_mutex release");
this->updateAggregatedStackPath();
}
void CpOdomTracker::pushPath() { this->pushPath(std::string()); }
void CpOdomTracker::pushPath(std::string pathname)
{
RCLCPP_INFO(getLogger(), "odom_tracker m_mutex acquire, to push path");
std::lock_guard<std::mutex> lock(m_mutex_);
this->logStateString(false);
if (this->odomSub_ != nullptr && this->odomSub_->get_publisher_count() == 0)
{
RCLCPP_ERROR_STREAM(
getLogger(),
"[CpOdomTracker] Odom tracker cannot record the path because there is no publisher to the "
"odometry topic");
}
pathInfos_.push_back({currentPathName_, this->currentMotionGoal_});
pathStack_.push_back(baseTrajectory_);
geometry_msgs::msg::PoseStamped goalPose;
if (this->currentMotionGoal_) goalPose = *this->currentMotionGoal_;
RCLCPP_INFO_STREAM(
getLogger(), "[CpOdomTracker] currentPathName: " << pathname
<< " size: " << baseTrajectory_.poses.size()
<< " current motion goal: " << goalPose);
currentPathName_ = pathname;
// clean the current trajectory to start a new one
baseTrajectory_.poses.clear();
this->logStateString(false);
this->updateAggregatedStackPath();
this->currentMotionGoal_.reset();
}
void CpOdomTracker::popPath(int popCount, bool keepPreviousPath)
{
RCLCPP_INFO(getLogger(), "odom_tracker m_mutex acquire to pop path");
std::lock_guard<std::mutex> lock(m_mutex_);
RCLCPP_INFO(getLogger(), "POP PATH ENTRY");
this->logStateString();
if (this->odomSub_ != nullptr && this->odomSub_->get_publisher_count() == 0)
{
RCLCPP_ERROR_STREAM(
getLogger(),
"[CpOdomTracker] Odom tracker cannot record the path because there is no publisher to the "
"odometry topic");
}
if (!keepPreviousPath)
{
baseTrajectory_.poses.clear();
}
while (popCount > 0 && !pathStack_.empty())
{
auto & stackedPath = pathStack_.back().poses;
auto & stackedPathInfo = pathInfos_.back();
baseTrajectory_.poses.insert(
baseTrajectory_.poses.begin(), stackedPath.begin(), stackedPath.end());
this->currentMotionGoal_ = stackedPathInfo.goalPose;
pathStack_.pop_back();
pathInfos_.pop_back();
popCount--;
RCLCPP_INFO(getLogger(), "POP PATH Iteration ");
this->logStateString();
}
RCLCPP_INFO(getLogger(), "POP PATH EXITING");
this->logStateString();
RCLCPP_INFO(getLogger(), "odom_tracker m_mutex release");
this->updateAggregatedStackPath();
this->currentMotionGoal_.reset();
}
void CpOdomTracker::logStateString(bool debug)
{
std::stringstream ss;
ss << "--- odom tracker state ---" << std::endl;
ss
<< " - path stack -" << currentPathName_ << " - size:"
<< pathStack_.size()
//<< " goal: " << (getCurrentMotionGoal()? getCurrentMotionGoal()->pose: geometry_msgs::msg::Pose())
<< std::endl;
ss << " - [STACK-HEAD active path size: " << baseTrajectory_.poses.size() << "]" << std::endl;
int i = 0;
for (auto & p : pathStack_ | boost::adaptors::reversed)
{
auto & pathinfo = pathInfos_[pathInfos_.size() - i - 1];
std::string goalstr = "[]";
if (pathinfo.goalPose)
{
std::stringstream ss;
ss << *(pathinfo.goalPose);
goalstr = ss.str();
}
ss << " - p " << i << "[" << p.header.stamp << "], size: " << p.poses.size() << " - "
<< pathinfo.name << " - goal: " << goalstr << std::endl;
i++;
}
ss << "---";
if (this->odomSub_ != nullptr && this->odomSub_->get_publisher_count() == 0)
{
ss << std::endl
<< "[CpOdomTracker] Odom tracker cannot record the path because there is no publisher to "
"the odometry topic"
<< std::endl;
}
if (debug)
RCLCPP_DEBUG(getLogger(), ss.str().c_str());
else
RCLCPP_INFO(getLogger(), ss.str().c_str());
}
void CpOdomTracker::clearPath()
{
RCLCPP_INFO(getLogger(), "odom_tracker m_mutex acquire to clear path");
std::lock_guard<std::mutex> lock(m_mutex_);
baseTrajectory_.poses.clear();
rtPublishPaths(getNode()->now());
this->logStateString();
this->updateAggregatedStackPath();
this->currentMotionGoal_.reset();
}
void CpOdomTracker::setCurrentPathName(const std::string & currentPathName)
{
RCLCPP_INFO_STREAM(getLogger(), "[CpOdomTracker] set current path name: " << currentPathName);
currentPathName_ = currentPathName;
}
void CpOdomTracker::setStartPoint(const geometry_msgs::msg::PoseStamped & pose)
{
std::lock_guard<std::mutex> lock(m_mutex_);
RCLCPP_INFO_STREAM(getLogger(), "[CpOdomTracker] set current path starting point: " << pose);
if (baseTrajectory_.poses.size() > 0)
{
baseTrajectory_.poses[0] = pose;
}
else
{
baseTrajectory_.poses.push_back(pose);
}
this->updateAggregatedStackPath();
}
void CpOdomTracker::setStartPoint(const geometry_msgs::msg::Pose & pose)
{
std::lock_guard<std::mutex> lock(m_mutex_);
RCLCPP_INFO_STREAM(getLogger(), "[CpOdomTracker] set current path starting point: " << pose);
geometry_msgs::msg::PoseStamped posestamped;
posestamped.header.frame_id = this->odomFrame_;
posestamped.header.stamp = getNode()->now();
posestamped.pose = pose;
if (baseTrajectory_.poses.size() > 0)
{
baseTrajectory_.poses[0] = posestamped;
}
else
{
baseTrajectory_.poses.push_back(posestamped);
}
this->updateAggregatedStackPath();
}
void CpOdomTracker::setCurrentMotionGoal(const geometry_msgs::msg::PoseStamped & pose)
{
std::lock_guard<std::mutex> lock(m_mutex_);
this->currentMotionGoal_ = pose;
}
std::optional<geometry_msgs::msg::PoseStamped> CpOdomTracker::getCurrentMotionGoal()
{
std::lock_guard<std::mutex> lock(m_mutex_);
return this->currentMotionGoal_;
}
nav_msgs::msg::Path CpOdomTracker::getPath()
{
std::lock_guard<std::mutex> lock(m_mutex_);
return this->baseTrajectory_;
}
/**
******************************************************************************************************************
* rtPublishPaths()
******************************************************************************************************************
*/
void CpOdomTracker::rtPublishPaths(rclcpp::Time timestamp)
{
baseTrajectory_.header.stamp = timestamp;
robotBasePathPub_->publish(baseTrajectory_);
aggregatedStackPathMsg_.header.stamp = timestamp;
robotBasePathStackedPub_->publish(aggregatedStackPathMsg_);
}
void CpOdomTracker::updateAggregatedStackPath()
{
aggregatedStackPathMsg_.poses.clear();
for (auto & p : pathStack_)
{
aggregatedStackPathMsg_.poses.insert(
aggregatedStackPathMsg_.poses.end(), p.poses.begin(), p.poses.end());
}
aggregatedStackPathMsg_.header.frame_id = this->odomFrame_;
}
/**
******************************************************************************************************************
* updateBackward()
******************************************************************************************************************
*/
bool CpOdomTracker::updateClearPath(const geometry_msgs::msg::PoseStamped & base_pose)
{
// we initially accept any message if the queue is empty
/// Track robot base pose
baseTrajectory_.header = base_pose.header;
bool acceptBackward = false;
bool clearingError = false;
bool finished = false;
while (!finished)
{
if (
baseTrajectory_.poses.size() <=
1) // we at least keep always the first point of the forward path when clearing
// (this is important for backwards planner replanning and not losing the
// last goal)
{
acceptBackward = false;
finished = true;
}
else
{
auto & carrotPose = baseTrajectory_.poses.back().pose;
auto & carrotPoint = carrotPose.position;
double carrotAngle = tf2::getYaw(carrotPose.orientation);
auto & currePose = base_pose.pose;
auto & currePoint = currePose.position;
double currentAngle = tf2::getYaw(currePose.orientation);
double lastpointdist = p2pDistance(carrotPoint, currePoint);
double goalAngleOffset = fabs(angles::shortest_angular_distance(carrotAngle, currentAngle));
acceptBackward = !baseTrajectory_.poses.empty() &&
lastpointdist < clearPointDistanceThreshold_ &&
goalAngleOffset < clearAngularDistanceThreshold_;
clearingError = lastpointdist > 2 * clearPointDistanceThreshold_;
RCLCPP_DEBUG_STREAM(
getLogger(), "[CpOdomTracker] clearing (accepted: " << acceptBackward
<< ") linerr: " << lastpointdist
<< ", anglerr: " << goalAngleOffset);
}
// RCLCPP_INFO(getLogger(),"Backwards, last distance: %lf < %lf accept: %d", dist,
// minPointDistanceBackwardThresh_, acceptBackward);
if (
acceptBackward &&
baseTrajectory_.poses.size() > 1) /* we always leave at least one item, specially interesting
for the backward local planner reach
the backwards goal with enough precision*/
{
baseTrajectory_.poses.pop_back();
}
else if (clearingError)
{
finished = true;
RCLCPP_WARN(getLogger(), "[CpOdomTracker] Incorrect odom clearing motion.");
}
else
{
finished = true;
/// Not removing point because it is enough far from the last cord point
}
}
return acceptBackward;
}
/**
******************************************************************************************************************
* updateRecordPath()
******************************************************************************************************************
*/
bool CpOdomTracker::updateRecordPath(const geometry_msgs::msg::PoseStamped & base_pose)
{
/// Track robot base pose
baseTrajectory_.header = base_pose.header;
bool enqueueOdomMessage = false;
double dist = -1;
if (baseTrajectory_.poses.empty())
{
enqueueOdomMessage = true;
}
else
{
const auto & prevPose = baseTrajectory_.poses.back().pose;
const geometry_msgs::msg::Point & prevPoint = prevPose.position;
double prevAngle = tf2::getYaw(prevPose.orientation);
const geometry_msgs::msg::Point & currePoint = base_pose.pose.position;
double currentAngle = tf2::getYaw(base_pose.pose.orientation);
dist = p2pDistance(prevPoint, currePoint);
double goalAngleOffset = fabs(angles::shortest_angular_distance(prevAngle, currentAngle));
// RCLCPP_WARN(getLogger(),"dist %lf vs min %lf", dist, recordPointDistanceThreshold_);
RCLCPP_WARN_THROTTLE(getLogger(), *getNode()->get_clock(), 2000, "odom received");
if (dist > recordPointDistanceThreshold_ || goalAngleOffset > recordAngularDistanceThreshold_)
{
enqueueOdomMessage = true;
}
else
{
// RCLCPP_WARN(getLogger(),"skip odom, dist: %lf", dist);
enqueueOdomMessage = false;
}
}
if (enqueueOdomMessage)
{
RCLCPP_WARN_THROTTLE(
getLogger(), *getNode()->get_clock(), 2000, "enqueue odom tracker pose. dist %lf vs min %lf",
dist, recordPointDistanceThreshold_);
baseTrajectory_.poses.push_back(base_pose);
}
return enqueueOdomMessage;
}
/**
******************************************************************************************************************
* reconfigCB()
******************************************************************************************************************
*/
void CpOdomTracker::updateConfiguration()
{
return;
if (!getNode()->get_parameter("odom_frame", this->odomFrame_))
{
}
if (!getNode()->get_parameter("record_point_distance_threshold", recordPointDistanceThreshold_))
{
}
if (!getNode()->get_parameter(
"record_angular_distance_threshold", recordAngularDistanceThreshold_))
{
}
if (!getNode()->get_parameter("clear_point_distance_threshold", clearPointDistanceThreshold_))
{
}
if (!getNode()->get_parameter("clear_angular_distance_threshold", clearAngularDistanceThreshold_))
{
}
}
/**
******************************************************************************************************************
* update()
******************************************************************************************************************
*/
void CpOdomTracker::update()
{
RCLCPP_INFO(getLogger(), "odom_tracker update");
auto pose = robotPose_->toPoseStampedMsg();
RCLCPP_INFO(getLogger(), "odom tracker updatd pose, frame: %s", pose.header.frame_id.c_str());
processNewPose(pose);
RCLCPP_INFO(getLogger(), "odom_tracker update end");
}
/**
******************************************************************************************************************
* odomMessageCallback()
******************************************************************************************************************
*/
void CpOdomTracker::odomMessageCallback(const nav_msgs::msg::Odometry::SharedPtr odom)
{
geometry_msgs::msg::PoseStamped base_pose;
base_pose.pose = odom->pose.pose;
base_pose.header = odom->header;
processNewPose(base_pose);
}
/**
******************************************************************************************************************
* processNewPose()
******************************************************************************************************************
*/
void CpOdomTracker::processNewPose(const geometry_msgs::msg::PoseStamped & odom)
{
RCLCPP_INFO_THROTTLE(
getLogger(), *getNode()->get_clock(), 5000,
"[odom_tracker] processing odom msg update heartbeat");
std::lock_guard<std::mutex> lock(m_mutex_);
updateConfiguration();
if (workingMode_ == WorkingMode::RECORD_PATH)
{
updateRecordPath(odom);
}
else if (workingMode_ == WorkingMode::CLEAR_PATH)
{
updateClearPath(odom);
}
// RCLCPP_WARN(getLogger(),"odomTracker odometry callback");
if (publishMessages)
{
rtPublishPaths(odom.header.stamp);
}
// RCLCPP_INFO(getLogger(),"odom_tracker m_mutex release");
}
} // namespace odom_tracker
} // namespace cl_nav2z

123
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/src/cl_nav2z/components/odom_tracker/cp_odom_tracker_node.cpp vendored
View File

@@ -1,123 +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 <actionlib/server/simple_action_server.h>
#include <odom_tracker/CpOdomTrackerAction.h>
#include <cl_nav2z/components/odom_tracker/cp_odom_tracker.hpp>
#include <memory>
typedef actionlib::SimpleActionServer<odom_tracker::CpOdomTrackerAction> Server;
using namespace odom_tracker;
using namespace cl_nav2z::odom_tracker;
class CpOdomTrackerActionServer
{
public:
std::shared_ptr<Server> as_;
CpOdomTracker odomTracker;
CpOdomTrackerActionServer() : odomTracker("move_base") {}
/**
******************************************************************************************************************
* execute()
******************************************************************************************************************
*/
void execute(
const CpOdomTrackerGoalConstPtr & goal) // Note: "Action" is not appended to DoDishes here
{
try
{
switch (goal->command)
{
case CpOdomTrackerGoal::RECORD_PATH:
odomTracker.setWorkingMode(WorkingMode::RECORD_PATH);
break;
case CpOdomTrackerGoal::CLEAR_PATH:
odomTracker.setWorkingMode(WorkingMode::CLEAR_PATH);
break;
case CpOdomTrackerGoal::IDLE:
odomTracker.setWorkingMode(WorkingMode::IDLE);
break;
case CpOdomTrackerGoal::START_BROADCAST_PATH:
odomTracker.setPublishMessages(true);
break;
case CpOdomTrackerGoal::STOP_BROADCAST_PATH:
odomTracker.setPublishMessages(false);
break;
case CpOdomTrackerGoal::PUSH_PATH:
odomTracker.pushPath();
break;
case CpOdomTrackerGoal::POP_PATH:
odomTracker.popPath();
break;
default:
RCLCPP_ERROR(
getLogger(), "Odom Tracker Node - Action Server execute error: incorrect command - %d",
goal->command);
as_->setAborted();
}
// never reach succeeded because were are interested in keeping the feedback alive
as_->setSucceeded();
}
catch (std::exception & ex)
{
RCLCPP_ERROR(getLogger(), "Odom Tracker Node - Action Server execute error: %s", ex.what());
as_->setAborted();
}
}
/**
******************************************************************************************************************
* run()
******************************************************************************************************************
*/
void run()
{
rclcpp::Node::SharedPtr n;
RCLCPP_INFO(getLogger(), "Creating odom tracker action server");
as_ = std::make_shared<Server>(
n, "odom_tracker",
std::bind(&CpOdomTrackerActionServer::execute, this, std::placeholders::_1), false);
RCLCPP_INFO(getLogger(), "Starting CpOdomTracker Action Server");
as_->start();
ros::spin();
}
};
int main(int argc, char ** argv)
{
ros::init(argc, argv, "odom_tracker_node");
CpOdomTrackerActionServer as;
as.run();
}

109
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/src/cl_nav2z/components/planner_switcher/cp_planner_switcher.cpp vendored
View File

@@ -1,109 +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 <cl_nav2z/cl_nav2z.hpp>
#include <cl_nav2z/components/planner_switcher/cp_planner_switcher.hpp>
namespace cl_nav2z
{
using namespace std::chrono_literals;
CpPlannerSwitcher::CpPlannerSwitcher() {}
void CpPlannerSwitcher::onInitialize()
{
rclcpp::QoS qos(rclcpp::KeepLast(1));
qos.transient_local().reliable();
this->planner_selector_pub_ =
getNode()->create_publisher<std_msgs::msg::String>("planner_selector", qos);
this->controller_selector_pub_ =
getNode()->create_publisher<std_msgs::msg::String>("controller_selector", qos);
}
void CpPlannerSwitcher::setDesiredGlobalPlanner(std::string plannerName)
{
desired_planner_ = plannerName;
}
void CpPlannerSwitcher::setDesiredController(std::string controllerName)
{
desired_controller_ = controllerName;
}
void CpPlannerSwitcher::setUndoPathBackwardPlanner(bool commit)
{
RCLCPP_INFO(getLogger(), "[CpPlannerSwitcher] Planner Switcher: Trying to set BackwardPlanner");
desired_planner_ = "UndoPathGlobalPlanner";
desired_controller_ = "BackwardLocalPlanner";
if (commit) commitPublish();
}
void CpPlannerSwitcher::setBackwardPlanner(bool commit)
{
RCLCPP_INFO(getLogger(), "[CpPlannerSwitcher] Planner Switcher: Trying to set BackwardPlanner");
desired_planner_ = "BackwardGlobalPlanner";
desired_controller_ = "BackwardLocalPlanner";
if (commit) commitPublish();
}
void CpPlannerSwitcher::setForwardPlanner(bool commit)
{
RCLCPP_INFO(getLogger(), "[CpPlannerSwitcher] Planner Switcher: Trying to set ForwardPlanner");
desired_planner_ = "ForwardGlobalPlanner";
desired_controller_ = "ForwardLocalPlanner";
if (commit) commitPublish();
}
void CpPlannerSwitcher::setPureSpinningPlanner(bool commit)
{
RCLCPP_INFO(
getLogger(), "[CpPlannerSwitcher] Planner Switcher: Trying to set PureSpinningPlanner");
desired_planner_ = "ForwardGlobalPlanner";
desired_controller_ = "PureSpinningLocalPlanner";
if (commit) commitPublish();
}
void CpPlannerSwitcher::setDefaultPlanners(bool commit)
{
desired_planner_ = "GridBased";
desired_controller_ = "FollowPath";
if (commit) commitPublish();
}
void CpPlannerSwitcher::commitPublish()
{
std_msgs::msg::String planner_msg;
planner_msg.data = desired_planner_;
this->planner_selector_pub_->publish(planner_msg);
std_msgs::msg::String controller_msg;
controller_msg.data = desired_controller_;
this->controller_selector_pub_->publish(controller_msg);
}
} // namespace cl_nav2z

170
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/src/cl_nav2z/components/pose/cp_pose.cpp vendored
View File

@@ -1,170 +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 <cl_nav2z/components/pose/cp_pose.hpp>
#include <memory>
#include <string>
namespace cl_nav2z
{
using namespace std::chrono_literals;
// static
std::shared_ptr<tf2_ros::TransformListener> Pose::tfListener_;
std::shared_ptr<tf2_ros::Buffer> Pose::tfBuffer_;
std::mutex Pose::listenerMutex_;
Pose::Pose(std::string targetFrame, std::string referenceFrame)
: isInitialized(false), poseFrameName_(targetFrame), referenceFrame_(referenceFrame)
{
this->pose_.header.frame_id = referenceFrame_;
}
std::string referenceFrameToString(StandardReferenceFrames referenceFrame)
{
switch (referenceFrame)
{
case StandardReferenceFrames::Map:
return "map";
case StandardReferenceFrames::Odometry:
return "odom";
default:
return "odom";
}
}
Pose::Pose(StandardReferenceFrames referenceFrame)
: Pose("base_link", referenceFrameToString(referenceFrame))
{
}
void Pose::onInitialize()
{
RCLCPP_INFO(
getLogger(), "[Pose] Creating Pose tracker component to track %s in the reference frame %s",
poseFrameName_.c_str(), referenceFrame_.c_str());
{
// singleton
std::lock_guard<std::mutex> guard(listenerMutex_);
if (tfListener_ == nullptr)
{
tfBuffer_ = std::make_shared<tf2_ros::Buffer>(getNode()->get_clock());
tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
}
}
}
void Pose::waitTransformUpdate(rclcpp::Rate r)
{
bool found = false;
RCLCPP_INFO(getLogger(), "[Pose Component] waitTransformUpdate");
while (rclcpp::ok() && !found)
{
tf2::Stamped<tf2::Transform> transform;
try
{
{
RCLCPP_INFO_THROTTLE(
getLogger(), *(getNode()->get_clock()), 1000,
"[Pose Component] waiting transform %s -> %s", referenceFrame_.c_str(),
poseFrameName_.c_str());
std::lock_guard<std::mutex> lock(listenerMutex_);
auto transformstamped =
tfBuffer_->lookupTransform(referenceFrame_, poseFrameName_, getNode()->now());
tf2::fromMsg(transformstamped, transform);
}
{
std::lock_guard<std::mutex> guard(m_mutex_);
tf2::toMsg(transform, this->pose_.pose);
this->pose_.header.stamp = tf2_ros::toRclcpp(transform.stamp_);
this->pose_.header.frame_id = referenceFrame_;
found = true;
this->isInitialized = true;
}
}
catch (tf2::TransformException & ex)
{
RCLCPP_ERROR_STREAM_THROTTLE(
getLogger(), *(getNode()->get_clock()), 1000,
"[Component pose] (" << poseFrameName_ << "/[" << referenceFrame_
<< "] ) is failing on pose update : " << ex.what());
}
r.sleep();
}
RCLCPP_INFO(getLogger(), "[Pose Component] waitTransformUpdate -> pose found!");
}
float Pose::getYaw() { return tf2::getYaw(pose_.pose.orientation); }
float Pose::getX() { return pose_.pose.position.x; }
float Pose::getY() { return pose_.pose.position.y; }
float Pose::getZ() { return pose_.pose.position.z; }
void Pose::update()
{
tf2::Stamped<tf2::Transform> transform;
try
{
if (!frozenReferenceFrameTime)
{
std::lock_guard<std::mutex> lock(listenerMutex_);
RCLCPP_INFO(
getLogger(), "[pose] looking up transform: %s -> %s", referenceFrame_.c_str(),
poseFrameName_.c_str());
auto transformstamped =
tfBuffer_->lookupTransform(referenceFrame_, poseFrameName_, rclcpp::Time(0));
tf2::fromMsg(transformstamped, transform);
RCLCPP_INFO(getLogger(), "[pose] transform found");
}
else
{
RCLCPP_INFO(
getLogger(), "[pose] looking up transform: %s -> %s", referenceFrame_.c_str(),
poseFrameName_.c_str());
auto transformstamped = tfBuffer_->lookupTransform(
referenceFrame_, *frozenReferenceFrameTime, poseFrameName_, rclcpp::Time(0),
referenceFrame_, 1s);
tf2::fromMsg(transformstamped, transform);
RCLCPP_INFO(getLogger(), "[pose] transform found");
}
{
std::lock_guard<std::mutex> guard(m_mutex_);
tf2::toMsg(transform, this->pose_.pose);
this->pose_.header.stamp = tf2_ros::toRclcpp(transform.stamp_);
this->pose_.header.frame_id = referenceFrame_;
this->isInitialized = true;
}
}
catch (tf2::TransformException & ex)
{
// RCLCPP_DEBUG(getLogger(), "[pose] EXCEPTION");
RCLCPP_ERROR_STREAM_THROTTLE(
getLogger(), *(getNode()->get_clock()), 1000,
"[Component pose] (" << poseFrameName_ << "/[" << referenceFrame_
<< "] ) is failing on pose update : " << ex.what());
}
}
} // namespace cl_nav2z

40
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/src/cl_nav2z/components/slam_toolbox/cp_slam_toolbox.cpp vendored
View File

@@ -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
*
******************************************************************************************************************/
#include <cl_nav2z/components/slam_toolbox/cp_slam_toolbox.hpp>
namespace cl_nav2z
{
CpSlamToolbox::CpSlamToolbox() : state_(SlamToolboxState::Resumed) {}
CpSlamToolbox::~CpSlamToolbox() {}
void CpSlamToolbox::toggleState()
{
if (state_ == SlamToolboxState::Paused)
{
state_ = SlamToolboxState::Resumed;
}
else
{
state_ = SlamToolboxState::Paused;
}
}
} // namespace cl_nav2z

30
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/src/cl_nav2z/components/waypoints_navigator/cp_waypoints_event_dispatcher.cpp vendored
View File

@@ -1,30 +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 <cl_nav2z/components/waypoints_navigator/cp_waypoints_event_dispatcher.hpp>
namespace cl_nav2z
{
void WaypointEventDispatcher::postWaypointEvent(int index)
{
auto & fn = postWaypointFn[index % WAYPOINTS_EVENTCOUNT];
if (fn != nullptr) fn();
}
} // namespace cl_nav2z

579
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/src/cl_nav2z/components/waypoints_navigator/cp_waypoints_navigator.cpp vendored
View File

@@ -1,579 +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 <tf2/transform_datatypes.h>
#include <yaml-cpp/yaml.h>
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <cl_nav2z/cl_nav2z.hpp>
#include <cl_nav2z/common.hpp>
#include <cl_nav2z/components/goal_checker_switcher/cp_goal_checker_switcher.hpp>
#include <cl_nav2z/components/odom_tracker/cp_odom_tracker.hpp>
#include <cl_nav2z/components/planner_switcher/cp_planner_switcher.hpp>
#include <cl_nav2z/components/pose/cp_pose.hpp>
#include <cl_nav2z/components/waypoints_navigator/cp_waypoints_navigator.hpp>
#include <fstream>
#include <rclcpp/rclcpp.hpp>
namespace cl_nav2z
{
using namespace std::chrono_literals;
CpWaypointNavigatorBase::CpWaypointNavigatorBase() : currentWaypoint_(0), waypoints_(0) {}
CpWaypointNavigatorBase::~CpWaypointNavigatorBase() {}
CpWaypointNavigator::CpWaypointNavigator() {}
void CpWaypointNavigatorBase::onInitialize() {}
void CpWaypointNavigator::onInitialize()
{
client_ = dynamic_cast<ClNav2Z *>(owner_);
this->requiresComponent(nav2ActionInterface_);
}
void CpWaypointNavigator::onGoalCancelled(
const components::CpNav2ActionInterface::WrappedResult & /*res*/)
{
stopWaitingResult();
this->onNavigationRequestCancelled();
}
void CpWaypointNavigator::onGoalAborted(
const components::CpNav2ActionInterface::WrappedResult & /*res*/)
{
stopWaitingResult();
this->onNavigationRequestAborted();
}
void CpWaypointNavigator::onGoalReached(
const components::CpNav2ActionInterface::WrappedResult & /*res*/)
{
waypointsEventDispatcher.postWaypointEvent(currentWaypoint_);
currentWaypoint_++;
RCLCPP_WARN(
getLogger(), "[CpWaypointNavigator] Goal result received, incrementing waypoint index: %ld",
currentWaypoint_);
stopWaitingResult();
this->notifyGoalReached();
onNavigationRequestSucceded();
}
void CpWaypointNavigatorBase::rewind(int /*count*/)
{
currentWaypoint_--;
if (currentWaypoint_ < 0) currentWaypoint_ = 0;
}
void CpWaypointNavigatorBase::forward(int /*count*/)
{
currentWaypoint_++;
if (currentWaypoint_ >= (long)waypoints_.size() - 1)
currentWaypoint_ = (long)waypoints_.size() - 1;
}
void CpWaypointNavigatorBase::seekName(std::string name)
{
bool found = false;
auto previousWaypoint = currentWaypoint_;
while (!found && currentWaypoint_ < (long)waypoints_.size())
{
auto & nextName = waypointsNames_[currentWaypoint_];
RCLCPP_INFO(
getLogger(), "[CpWaypointNavigator] seeking ,%ld/%ld candidate waypoint: %s",
currentWaypoint_, waypoints_.size(), nextName.c_str());
if (name == nextName)
{
found = true;
RCLCPP_INFO(
getLogger(), "[CpWaypointNavigator] found target waypoint: %s == %s-> found",
nextName.c_str(), name.c_str());
}
else
{
RCLCPP_INFO(
getLogger(), "[CpWaypointNavigator] current waypoint: %s != %s -> forward",
nextName.c_str(), name.c_str());
currentWaypoint_++;
}
}
if (found)
{
if (currentWaypoint_ >= (long)waypoints_.size() - 1)
currentWaypoint_ = (long)waypoints_.size() - 1;
}
else // search backwards
{
currentWaypoint_ = previousWaypoint;
while (!found && currentWaypoint_ > 0)
{
auto & nextName = waypointsNames_[currentWaypoint_];
RCLCPP_INFO(
getLogger(), "[CpWaypointNavigator] seeking , candidate waypoint: %s", nextName.c_str());
if (name == nextName)
{
found = true;
RCLCPP_INFO(
getLogger(), "[CpWaypointNavigator] found target waypoint: %s == %s-> found",
nextName.c_str(), name.c_str());
}
else
{
RCLCPP_INFO(
getLogger(), "[CpWaypointNavigator] current waypoint: %s != %s -> rewind",
nextName.c_str(), name.c_str());
currentWaypoint_--;
}
}
}
RCLCPP_INFO(
getLogger(), "[CpWaypointNavigator] seekName( %s), previous index: %ld, after index: %ld",
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;
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 =
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());
}
else
{
RCLCPP_ERROR(getLogger(), "waypoints plan file not found: NONE");
}
}
void CpWaypointNavigator::stopWaitingResult()
{
if (succeddedNav2ZClientConnection_.connected())
{
this->succeddedNav2ZClientConnection_.disconnect();
this->cancelledNav2ZClientConnection_.disconnect();
this->abortedNav2ZClientConnection_.disconnect();
}
}
std::optional<std::shared_future<
std::shared_ptr<rclcpp_action::ClientGoalHandle<nav2_msgs::action::NavigateToPose>>>>
CpWaypointNavigator::sendNextGoal(std::optional<NavigateNextWaypointOptions> options)
{
if (currentWaypoint_ >= 0 && currentWaypoint_ < (int)waypoints_.size())
{
auto & next = waypoints_[currentWaypoint_];
std::string nextName;
if ((long)waypointsNames_.size() > currentWaypoint_)
{
nextName = waypointsNames_[currentWaypoint_];
RCLCPP_INFO(
getLogger(), "[CpWaypointNavigator] sending goal, waypoint: %s", nextName.c_str());
}
else
{
RCLCPP_INFO(
getLogger(), "[CpWaypointNavigator] sending goal, waypoint: %ld", currentWaypoint_);
}
nav2_msgs::action::NavigateToPose::Goal goal;
auto p = client_->getComponent<cl_nav2z::Pose>();
auto pose = p->toPoseMsg();
// configuring goal
goal.pose.header.frame_id = p->getReferenceFrame();
//goal.pose.header.stamp = getNode()->now();
goal.pose.pose = next;
auto plannerSwitcher = client_->getComponent<CpPlannerSwitcher>();
plannerSwitcher->setDefaultPlanners(false);
if (options && options->controllerName_)
{
RCLCPP_WARN(
getLogger(), "[WaypointsNavigator] override controller: %s",
options->controllerName_->c_str());
plannerSwitcher->setDesiredController(*options->controllerName_);
}
else
{
RCLCPP_WARN(getLogger(), "[WaypointsNavigator] Configuring default planners");
}
auto goalCheckerSwitcher = client_->getComponent<CpGoalCheckerSwitcher>();
if (options && options->goalCheckerName_)
{
RCLCPP_WARN(
getLogger(), "[WaypointsNavigator] override goal checker: %s",
options->goalCheckerName_->c_str());
goalCheckerSwitcher->setGoalCheckerId(*options->goalCheckerName_);
}
else
{
RCLCPP_WARN(getLogger(), "[WaypointsNavigator] Configuring default goal checker");
goalCheckerSwitcher->setGoalCheckerId("goal_checker");
}
plannerSwitcher->commitPublish();
// publish stuff
// rclcpp::sleep_for(5s);
RCLCPP_INFO(getLogger(), "[WaypointsNavigator] Getting odom tracker");
auto odomTracker = client_->getComponent<cl_nav2z::odom_tracker::CpOdomTracker>();
if (odomTracker != nullptr)
{
RCLCPP_INFO(getLogger(), "[WaypointsNavigator] Storing path in odom tracker");
auto pathname = this->owner_->getStateMachine()->getCurrentState()->getName() + " - " +
getName() + " - " + nextName;
odomTracker->pushPath(pathname);
odomTracker->setStartPoint(pose);
odomTracker->setWorkingMode(cl_nav2z::odom_tracker::WorkingMode::RECORD_PATH);
}
// SEND GOAL
// if (!succeddedNav2ZClientConnection_.connected())
// {
// this->succeddedNav2ZClientConnection_ =
// client_->onSucceeded(&WaypointNavigator::onGoalReached, this);
// this->cancelledNav2ZClientConnection_ =
// client_->onAborted(&WaypointNavigator::onGoalCancelled, this);
// this->abortedNav2ZClientConnection_ =
// client_->onCancelled(&WaypointNavigator::onGoalAborted, this);
// }
// Set up navigation result handling
if (!succeddedNav2ZClientConnection_.connected())
{
succeddedNav2ZClientConnection_ =
nav2ActionInterface_->onNavigationSucceeded(&CpWaypointNavigator::onGoalReached, this);
abortedNav2ZClientConnection_ =
nav2ActionInterface_->onNavigationAborted(&CpWaypointNavigator::onGoalAborted, this);
cancelledNav2ZClientConnection_ =
nav2ActionInterface_->onNavigationCancelled(&CpWaypointNavigator::onGoalCancelled, this);
}
return nav2ActionInterface_->sendGoal(goal);
}
else
{
RCLCPP_WARN(
getLogger(),
"[CpWaypointsNavigator] All waypoints were consumed. There is no more waypoints available.");
}
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<EvWaypointFinal>();
}
}
void CpWaypointNavigator::onNavigationResult(
const components::CpNav2ActionInterface::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 CpWaypointNavigatorBase::insertWaypoint(int index, geometry_msgs::msg::Pose & newpose)
{
if (index >= 0 && index <= (int)waypoints_.size())
{
waypoints_.insert(waypoints_.begin(), index, newpose);
}
}
void CpWaypointNavigatorBase::setWaypoints(const std::vector<geometry_msgs::msg::Pose> & waypoints)
{
this->waypoints_ = waypoints;
}
void CpWaypointNavigatorBase::setWaypoints(const std::vector<Pose2D> & waypoints)
{
waypoints_.clear();
waypointsNames_.clear();
int i = 0;
for (auto & p : waypoints)
{
geometry_msgs::msg::Pose pose;
pose.position.x = p.x_;
pose.position.y = p.y_;
pose.position.z = 0.0;
tf2::Quaternion q;
q.setRPY(0, 0, p.yaw_);
pose.orientation = tf2::toMsg(q);
waypoints_.push_back(pose);
waypointsNames_.push_back(std::to_string(i++));
}
}
void CpWaypointNavigatorBase::removeWaypoint(int index)
{
if (index >= 0 && index < (int)waypoints_.size())
{
waypoints_.erase(waypoints_.begin() + index);
}
}
const std::vector<geometry_msgs::msg::Pose> & CpWaypointNavigatorBase::getWaypoints() const
{
return waypoints_;
}
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<geometry_msgs::msg::Pose> CpWaypointNavigatorBase::getNamedPose(
std::string name) const
{
if (this->waypointsNames_.size() > 0)
{
for (int i = 0; i < (int)this->waypointsNames_.size(); i++)
{
if (this->waypointsNames_[i] == name)
{
return this->waypoints_[i];
}
}
}
return std::nullopt;
}
const std::vector<std::string> & CpWaypointNavigatorBase::getWaypointNames() const
{
return waypointsNames_;
}
std::optional<std::string> CpWaypointNavigatorBase::getCurrentWaypointName() const
{
if (currentWaypoint_ >= 0 && currentWaypoint_ < (int)waypointsNames_.size())
{
return waypointsNames_[currentWaypoint_];
}
return std::nullopt;
}
long CpWaypointNavigatorBase::getCurrentWaypointIndex() const { return currentWaypoint_; }
#define HAVE_NEW_YAMLCPP
void CpWaypointNavigatorBase::loadWayPointsFromFile(std::string 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)
{
throw std::string("Waypoints file not found");
}
try
{
#ifdef HAVE_NEW_YAMLCPP
YAML::Node node = YAML::Load(ifs);
#else
YAML::Parser parser(ifs);
parser.GetNextDocument(node);
#endif
#ifdef HAVE_NEW_YAMLCPP
const YAML::Node & wp_node_tmp = node["waypoints"];
const YAML::Node * wp_node = wp_node_tmp ? &wp_node_tmp : NULL;
#else
const YAML::Node * wp_node = node.FindValue("waypoints");
#endif
if (wp_node != NULL)
{
for (int64_t i = 0; i < wp_node->size(); ++i)
{
// Parse waypoint entries on YAML
geometry_msgs::msg::Pose wp;
try
{
// (*wp_node)[i]["name"] >> wp.name;
// (*wp_node)[i]["frame_id"] >> wp.header.frame_id;
auto wpnodei = (*wp_node)[i];
wp.position.x = wpnodei["position"]["x"].as<double>();
wp.position.y = wpnodei["position"]["y"].as<double>();
wp.position.z = wpnodei["position"]["z"].as<double>();
wp.orientation.x = wpnodei["orientation"]["x"].as<double>();
wp.orientation.y = wpnodei["orientation"]["y"].as<double>();
wp.orientation.z = wpnodei["orientation"]["z"].as<double>();
wp.orientation.w = wpnodei["orientation"]["w"].as<double>();
if (wpnodei["name"].IsDefined())
{
this->waypointsNames_.push_back(wpnodei["name"].as<std::string>());
}
this->waypoints_.push_back(wp);
}
catch (...)
{
RCLCPP_ERROR(getLogger(), "parsing waypoint file, syntax error in point %ld", i);
}
}
RCLCPP_INFO_STREAM(getLogger(), "Parsed " << this->waypoints_.size() << " waypoints.");
}
else
{
RCLCPP_WARN_STREAM(getLogger(), "Couldn't find any waypoints in the provided yaml file.");
}
}
catch (const YAML::ParserException & ex)
{
RCLCPP_ERROR_STREAM(
getLogger(), "Error loading the Waypoints YAML file. Incorrect syntax: " << ex.what());
}
}
void CpWaypointNavigatorBase::loadWayPointsFromFile2(std::string filepath)
{
RCLCPP_INFO_STREAM(getLogger(), "[CpWaypointNavigator] Loading file:" << filepath);
this->waypoints_.clear();
std::ifstream ifs(filepath.c_str(), std::ifstream::in);
if (ifs.good() == false)
{
throw std::string("Waypoints file not found");
}
try
{
#ifdef HAVE_NEW_YAMLCPP
YAML::Node node = YAML::Load(ifs);
#else
YAML::Parser parser(ifs);
parser.GetNextDocument(node);
#endif
#ifdef HAVE_NEW_YAMLCPP
const YAML::Node & wp_node_tmp = node["waypoints"];
const YAML::Node * wp_node = wp_node_tmp ? &wp_node_tmp : NULL;
#else
const YAML::Node * wp_node = node.FindValue("waypoints");
#endif
if (wp_node != NULL)
{
for (int64_t i = 0; i < wp_node->size(); ++i)
{
// Parse waypoint entries on YAML
geometry_msgs::msg::Pose wp;
try
{
// (*wp_node)[i]["name"] >> wp.name;
// (*wp_node)[i]["frame_id"] >> wp.header.frame_id;
wp.position.x = (*wp_node)[i]["x"].as<double>();
wp.position.y = (*wp_node)[i]["y"].as<double>();
auto name = (*wp_node)[i]["name"].as<std::string>();
this->waypoints_.push_back(wp);
this->waypointsNames_.push_back(name);
}
catch (...)
{
RCLCPP_ERROR(getLogger(), "parsing waypoint file, syntax error in point %ld", i);
}
}
RCLCPP_INFO_STREAM(getLogger(), "Parsed " << this->waypoints_.size() << " waypoints.");
}
else
{
RCLCPP_WARN_STREAM(getLogger(), "Couldn't find any waypoints in the provided yaml file.");
}
}
catch (const YAML::ParserException & ex)
{
RCLCPP_ERROR_STREAM(
getLogger(), "Error loading the Waypoints YAML file. Incorrect syntax: " << ex.what());
}
}
} // namespace cl_nav2z

156
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_nav2z/src/cl_nav2z/components/waypoints_navigator/cp_waypoints_visualizer.cpp vendored
View File

@@ -1,156 +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 <cl_nav2z/components/waypoints_navigator/cp_waypoints_visualizer.hpp>
namespace cl_nav2z
{
const std::string frameid = "map";
CpWaypointsVisualizer::CpWaypointsVisualizer(rclcpp::Duration duration) : ISmaccUpdatable(duration)
{
}
void CpWaypointsVisualizer::onInitialize()
{
markersPub_ = getNode()->create_publisher<visualization_msgs::msg::MarkerArray>(
"cp_waypoints_visualizer/visualization_markers", rclcpp::QoS(rclcpp::KeepLast(1)));
this->requiresComponent(waypointsNavigator_);
auto & waypoints = waypointsNavigator_->getWaypoints();
auto & waypointsNames = waypointsNavigator_->getWaypointNames();
int i = 0;
for (auto & waypoint : waypoints)
{
std::string name;
if ((long)waypointsNames.size() > i)
{
name = waypointsNames[i];
}
else
{
name = "waypoint_" + std::to_string(i);
}
visualization_msgs::msg::Marker marker;
createMarker(waypoint, marker);
markers_.markers.push_back(marker);
visualization_msgs::msg::Marker markerlabel;
createMarkerLabel(waypoint, name, markerlabel);
markerLabels_.markers.push_back(markerlabel);
i++;
}
}
void CpWaypointsVisualizer::createMarkerLabel(
const geometry_msgs::msg::Pose & waypoint, std::string label,
visualization_msgs::msg::Marker & marker)
{
marker.header.frame_id = frameid;
marker.header.stamp = getNode()->now();
marker.ns = "waypoints_labels";
marker.id = markers_.markers.size();
marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
marker.action = visualization_msgs::msg::Marker::ADD;
marker.scale.x = 0.3;
marker.scale.y = 0.3;
marker.scale.z = 0.3;
marker.text = label;
marker.color.a = 1.0;
marker.pose = waypoint;
marker.pose.position.z += 0.3;
}
void CpWaypointsVisualizer::createMarker(
const geometry_msgs::msg::Pose & waypoint, visualization_msgs::msg::Marker & marker)
{
marker.header.frame_id = frameid;
marker.header.stamp = getNode()->now();
marker.ns = "waypoints";
marker.id = markers_.markers.size();
marker.type = visualization_msgs::msg::Marker::SPHERE;
marker.action = visualization_msgs::msg::Marker::ADD;
marker.scale.x = 0.1;
marker.scale.y = 0.1;
marker.scale.z = 0.1;
marker.color.a = 1.0;
marker.pose = waypoint;
}
void CpWaypointsVisualizer::update()
{
std::lock_guard<std::mutex> guard(m_mutex_);
auto index = waypointsNavigator_->getCurrentWaypointIndex();
int i = 0;
for (auto & marker : markers_.markers)
{
marker.header.stamp = getNode()->now();
if (i >= index)
{
marker.color.r = 1.0;
marker.color.g = 0;
marker.color.b = 0;
}
else
{
marker.color.r = 0.0;
marker.color.g = 1.0;
marker.color.b = 0;
}
i++;
}
i = 0;
for (auto & marker : markerLabels_.markers)
{
marker.header.stamp = getNode()->now();
if (i >= index)
{
marker.color.r = 1.0;
marker.color.g = 0;
marker.color.b = 0;
}
else
{
marker.color.r = 0.0;
marker.color.g = 1.0;
marker.color.b = 0;
}
i++;
}
//markers_.header.stamp = getNode()->now();
markersPub_->publish(markers_);
markersPub_->publish(markerLabels_);
}
} // namespace cl_nav2z

2399
src/thirdparty/SMACC2-humble/smacc2_client_library/cl_ros2_timer/CHANGELOG.rst vendored
View File

File diff suppressed because it is too large Load Diff

Some files were not shown because too many files have changed in this diff Show More