diff --git a/CMakeLists.txt b/CMakeLists.txt
index c4f0ca9cf96347bfa5d8d2c6353ff029f2ce4562..dc7409473211b1d09061b0a53091dfb0ebf7ca35 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -12,12 +12,14 @@ add_definitions(-std=c++11)
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
- roscpp
- dynamic_reconfigure
- diagnostic_updater
+ roscpp
+ dynamic_reconfigure
+ diagnostic_updater
behaviortree_cpp_v3
iri_behaviortree
tf2_ros
+ geometry_msgs
+ sensor_msgs
)
## System dependencies are found with CMake's conventions
@@ -82,6 +84,8 @@ catkin_python_setup()
# std_msgs # Or other packages containing msgs
# )
+set(head_helpers_name head_helpers)
+
###################################
## catkin specific configuration ##
###################################
@@ -93,8 +97,8 @@ catkin_python_setup()
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS include
- LIBRARIES ${PROJECT_NAME}
- CATKIN_DEPENDS roscpp dynamic_reconfigure diagnostic_updater behaviortree_cpp_v3 iri_behaviortree tf2_ros
+ LIBRARIES ${PROJECT_NAME} ${head_helpers_name}
+ CATKIN_DEPENDS roscpp dynamic_reconfigure diagnostic_updater behaviortree_cpp_v3 iri_behaviortree tf2_ros geometry_msgs sensor_msgs
# DEPENDS system_lib
DEPENDS Boost
)
@@ -114,8 +118,19 @@ include_directories(
# add_library(foo
# src/${PROJECT_NAME}/foo.cpp
# )
+
+### Add head_helpers_library
+add_library(${head_helpers_name}
+ src/head_helpers.cpp
+)
+
+target_link_libraries(${head_helpers_name} ${catkin_LIBRARIES})
+add_dependencies(${head_helpers_name} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
add_library(${PROJECT_NAME} include/${PROJECT_NAME}/iri_base_bt_client.h src/iri_bt_basic_nodes.cpp include/${PROJECT_NAME}/iri_bt_dyn_reconf.h)
set_target_properties(${PROJECT_NAME} PROPERTIES LINKER_LANGUAGE CXX)
+target_link_libraries(${PROJECT_NAME} ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINATION}/lib${head_helpers_name}.so)
+add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${head_helpers_name})
## Declare a cpp executable
# add_executable(foo_node src/foo_node.cpp)
diff --git a/include/head_helpers.h b/include/head_helpers.h
new file mode 100644
index 0000000000000000000000000000000000000000..b1172004a7b0c8f6a0018da41509e009778cbefd
--- /dev/null
+++ b/include/head_helpers.h
@@ -0,0 +1,153 @@
+// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
+// Author
+// All rights reserved.
+//
+// This file is part of iri-ros-pkg
+// iri-ros-pkg is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+// IMPORTANT NOTE: This code has been generated through a script from the
+// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
+// of the scripts. ROS topics can be easly add by using those scripts. Please
+// refer to the IRI wiki page for more information:
+// http://wikiri.upc.es/index.php/Robotics_Lab
+
+#ifndef _raadical_head_helpers_h_
+#define _raadical_head_helpers_h_
+
+#include "iri_bt_factory.h"
+#include <geometry_msgs/PoseStamped.h>
+#include <sensor_msgs/CameraInfo.h>
+#include <tf2_ros/transform_listener.h>
+
+/**
+ * \brief Register the conditions and actions defined in the library.
+ *
+ * \param _factory (IriBehaviorTreeFactory)
+ *
+ * \param _ns Namespace or prefix for the Bt names.
+ *
+ */
+void head_helpers_init(IriBehaviorTreeFactory &_factory, const std::string& _ns);
+
+/**
+ * \brief Function to set tf2_buffer pointer.
+ *
+ * \param _tf2_buffer tf2 buffer
+ *
+ */
+void head_helpers_set_tf2_buffer(tf2_ros::Buffer* _tf2_buffer);
+
+/**
+ * \brief Function to delete tf2_buffer pointer.
+ *
+ */
+void head_helpers_delete_tf2_buffer(void);
+
+/**
+ * \brief Function to set invert_pan variable.
+ *
+ * \param _invert Value to set.
+ *
+ */
+void head_helpers_set_invert_pan(bool _invert);
+
+/**
+ * \brief Function to set invert_pan variable.
+ *
+ * \param _invert Value to set.
+ *
+ */
+void head_helpers_set_invert_tilt(bool _invert);
+
+/**
+ * \brief Function to get pan and tilt angles from an image point.
+ *
+ * It has the following input ports:
+ *
+ * x (double): Image x coordinate.
+ *
+ * y (double): Image y coordinate.
+ *
+ * camera_info (sensor_msgs::CameraInfo): Camera info of the camera.
+ *
+ * It has the following output ports:
+ *
+ * pan (double): Pan angle to center the image on the target point.
+ *
+ * tilt (double): Tilt angle to center the image on the target point.
+ *
+ * \param _self node with the required ports:
+ *
+ * \return a BT:NodeStatus indicating whether the request has been
+ * successfull (BT::NodeStatus::SUCCESS) or not (BT::NodeStatus::FAILURE).
+ * If inputs are missing or incorrect it also returns BT::NodeStatus::FAILURE.
+ *
+ */
+BT::NodeStatus from_img_point_to_pan_tilt(BT::TreeNode& _self);
+
+/**
+ * \brief Function to get pan and tilt angles from an image point.
+ *
+ * \param _x Image x coordinate.
+ *
+ * \param _y Image y coordinate.
+ *
+ * \param _camera_info Camera info of the camera.
+ *
+ * \param _pan Pan angle to center the image on the target point.
+ *
+ * \param _tilt Tilt angle to center the image on the target point.
+ */
+void from_img_point_to_pan_tilt_c(double _x, double _y, const sensor_msgs::CameraInfo& _camera_info, double& _pan, double& _tilt);
+
+/**
+ * \brief Function to get pan and tilt angles from an image point.
+ *
+ * It has the following input ports:
+ *
+ * point (geometry_msgs::PoseStamped): Target world point.
+ *
+ * follower_frame (std::string): Frame that must follow the target point.
+ *
+ * It has the following output ports:
+ *
+ * pan (double): Pan angle to center the image on the target point.
+ *
+ * tilt (double): Tilt angle to center the image on the target point.
+ *
+ * \param _self node with the required ports:
+ *
+ * \return a BT:NodeStatus indicating whether the request has been
+ * successfull (BT::NodeStatus::SUCCESS) or not (BT::NodeStatus::FAILURE).
+ * If inputs are missing or incorrect it also returns BT::NodeStatus::FAILURE.
+ *
+ */
+BT::NodeStatus from_world_point_to_pan_tilt(BT::TreeNode& _self);
+
+/**
+ * \brief Function to get pan and tilt angles from an image point.
+ *
+ * \param _pose Target world point.
+ *
+ * \param _follower_frame Frame that must follow the target point.
+ *
+ * \param _pan Pan angle to center the image on the target point.
+ *
+ * \param _tilt Tilt angle to center the image on the target point.
+ *
+ * \return True if pan&tilt angles can be calculated.
+ */
+bool from_world_point_to_pan_tilt_c(const geometry_msgs::PoseStamped& _pose, const std::string& _follower_frame, double& _pan, double& _tilt);
+
+#endif
\ No newline at end of file
diff --git a/include/iri_base_bt_client/head_helpers.h b/include/iri_base_bt_client/head_helpers.h
new file mode 100644
index 0000000000000000000000000000000000000000..b1172004a7b0c8f6a0018da41509e009778cbefd
--- /dev/null
+++ b/include/iri_base_bt_client/head_helpers.h
@@ -0,0 +1,153 @@
+// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
+// Author
+// All rights reserved.
+//
+// This file is part of iri-ros-pkg
+// iri-ros-pkg is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+// IMPORTANT NOTE: This code has been generated through a script from the
+// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
+// of the scripts. ROS topics can be easly add by using those scripts. Please
+// refer to the IRI wiki page for more information:
+// http://wikiri.upc.es/index.php/Robotics_Lab
+
+#ifndef _raadical_head_helpers_h_
+#define _raadical_head_helpers_h_
+
+#include "iri_bt_factory.h"
+#include <geometry_msgs/PoseStamped.h>
+#include <sensor_msgs/CameraInfo.h>
+#include <tf2_ros/transform_listener.h>
+
+/**
+ * \brief Register the conditions and actions defined in the library.
+ *
+ * \param _factory (IriBehaviorTreeFactory)
+ *
+ * \param _ns Namespace or prefix for the Bt names.
+ *
+ */
+void head_helpers_init(IriBehaviorTreeFactory &_factory, const std::string& _ns);
+
+/**
+ * \brief Function to set tf2_buffer pointer.
+ *
+ * \param _tf2_buffer tf2 buffer
+ *
+ */
+void head_helpers_set_tf2_buffer(tf2_ros::Buffer* _tf2_buffer);
+
+/**
+ * \brief Function to delete tf2_buffer pointer.
+ *
+ */
+void head_helpers_delete_tf2_buffer(void);
+
+/**
+ * \brief Function to set invert_pan variable.
+ *
+ * \param _invert Value to set.
+ *
+ */
+void head_helpers_set_invert_pan(bool _invert);
+
+/**
+ * \brief Function to set invert_pan variable.
+ *
+ * \param _invert Value to set.
+ *
+ */
+void head_helpers_set_invert_tilt(bool _invert);
+
+/**
+ * \brief Function to get pan and tilt angles from an image point.
+ *
+ * It has the following input ports:
+ *
+ * x (double): Image x coordinate.
+ *
+ * y (double): Image y coordinate.
+ *
+ * camera_info (sensor_msgs::CameraInfo): Camera info of the camera.
+ *
+ * It has the following output ports:
+ *
+ * pan (double): Pan angle to center the image on the target point.
+ *
+ * tilt (double): Tilt angle to center the image on the target point.
+ *
+ * \param _self node with the required ports:
+ *
+ * \return a BT:NodeStatus indicating whether the request has been
+ * successfull (BT::NodeStatus::SUCCESS) or not (BT::NodeStatus::FAILURE).
+ * If inputs are missing or incorrect it also returns BT::NodeStatus::FAILURE.
+ *
+ */
+BT::NodeStatus from_img_point_to_pan_tilt(BT::TreeNode& _self);
+
+/**
+ * \brief Function to get pan and tilt angles from an image point.
+ *
+ * \param _x Image x coordinate.
+ *
+ * \param _y Image y coordinate.
+ *
+ * \param _camera_info Camera info of the camera.
+ *
+ * \param _pan Pan angle to center the image on the target point.
+ *
+ * \param _tilt Tilt angle to center the image on the target point.
+ */
+void from_img_point_to_pan_tilt_c(double _x, double _y, const sensor_msgs::CameraInfo& _camera_info, double& _pan, double& _tilt);
+
+/**
+ * \brief Function to get pan and tilt angles from an image point.
+ *
+ * It has the following input ports:
+ *
+ * point (geometry_msgs::PoseStamped): Target world point.
+ *
+ * follower_frame (std::string): Frame that must follow the target point.
+ *
+ * It has the following output ports:
+ *
+ * pan (double): Pan angle to center the image on the target point.
+ *
+ * tilt (double): Tilt angle to center the image on the target point.
+ *
+ * \param _self node with the required ports:
+ *
+ * \return a BT:NodeStatus indicating whether the request has been
+ * successfull (BT::NodeStatus::SUCCESS) or not (BT::NodeStatus::FAILURE).
+ * If inputs are missing or incorrect it also returns BT::NodeStatus::FAILURE.
+ *
+ */
+BT::NodeStatus from_world_point_to_pan_tilt(BT::TreeNode& _self);
+
+/**
+ * \brief Function to get pan and tilt angles from an image point.
+ *
+ * \param _pose Target world point.
+ *
+ * \param _follower_frame Frame that must follow the target point.
+ *
+ * \param _pan Pan angle to center the image on the target point.
+ *
+ * \param _tilt Tilt angle to center the image on the target point.
+ *
+ * \return True if pan&tilt angles can be calculated.
+ */
+bool from_world_point_to_pan_tilt_c(const geometry_msgs::PoseStamped& _pose, const std::string& _follower_frame, double& _pan, double& _tilt);
+
+#endif
\ No newline at end of file
diff --git a/package.xml b/package.xml
index ffbd00b6fb0e066f3672b17135578b9ca9c47a7b..b45eb3e3e49acc1d03aafc6518608074d2fbdb00 100644
--- a/package.xml
+++ b/package.xml
@@ -19,7 +19,9 @@
<depend>behaviortree_cpp_v3</depend>
<depend>iri_behaviortree</depend>
<depend>tf2_ros</depend>
-
+ <depend>sensor_msgs</depend>
+ <depend>geometry_msgs</depend>
+
<!-- The export tag contains other, unspecified, tags -->
<export>
diff --git a/src/head_helpers.cpp b/src/head_helpers.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ec8ecd590e17621bd175cb94ad2fdbac1542c02f
--- /dev/null
+++ b/src/head_helpers.cpp
@@ -0,0 +1,154 @@
+#include "head_helpers.h"
+#include <ros/ros.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2/utils.h>
+#include <math.h>
+
+std::shared_ptr<tf2_ros::Buffer> tf2_buffer_p;
+bool invert_pan;
+bool invert_tilt;
+
+void head_helpers_init(IriBehaviorTreeFactory &_factory, const std::string& _ns)
+{
+ BT::PortsList img_to_pt_ports = {BT::InputPort<double>("x"), BT::InputPort<double>("y"), BT::InputPort<sensor_msgs::CameraInfo>("camera_info"), BT::OutputPort<double>("pan"), BT::OutputPort<double>("tilt")};
+ BT::PortsList point_to_pt_ports = {BT::InputPort<geometry_msgs::PoseStamped>("pose"), BT::InputPort<std::string>("follower_frame"), BT::OutputPort<double>("pan"), BT::OutputPort<double>("tilt")};
+
+ _factory.registerSimpleAction(_ns+"from_img_point_to_pan_tilt", std::bind(&from_img_point_to_pan_tilt, std::placeholders::_1), img_to_pt_ports);
+ _factory.registerSimpleAction(_ns+"from_world_point_to_pan_tilt", std::bind(&from_world_point_to_pan_tilt, std::placeholders::_1), point_to_pt_ports);
+}
+
+void head_helpers_set_tf2_buffer(tf2_ros::Buffer* _tf2_buffer)
+{
+ tf2_buffer_p.reset(_tf2_buffer);
+}
+
+void head_helpers_delete_tf2_buffer(void)
+{
+ tf2_buffer_p.reset();
+}
+
+void head_helpers_set_invert_pan(bool _invert)
+{
+ invert_pan = _invert;
+}
+
+void head_helpers_set_invert_tilt(bool _invert)
+{
+ invert_tilt = _invert;
+}
+
+BT::NodeStatus from_img_point_to_pan_tilt(BT::TreeNode& _self)
+{
+ BT::Optional<double> x_p = _self.getInput<double>("x");
+ BT::Optional<double> y_p = _self.getInput<double>("y");
+ BT::Optional<sensor_msgs::CameraInfo> camera_info_p = _self.getInput<sensor_msgs::CameraInfo>("camera_info");
+ double x;
+ double y;
+ sensor_msgs::CameraInfo camera_info;
+ double pan;
+ double tilt;
+ int row = 0, col = 0, count = 0;
+ _self.setOutput("pan", pan);
+ _self.setOutput("tilt", tilt);
+ if (!x_p || !y_p || !camera_info_p)
+ {
+ ROS_ERROR_STREAM("head_helpers::from_img_point_to_pan_tilt-> Incorrect or missing input. It needs the following input ports:"
+ << " x (double), y (double) and camera_info (sensor_msgs::CameraInfo)");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ x = x_p.value();
+ y = y_p.value();
+ camera_info = camera_info_p.value();
+
+ from_img_point_to_pan_tilt_c(x, y, camera_info, pan, tilt);
+ _self.setOutput("pan", pan);
+ _self.setOutput("tilt", tilt);
+ return BT::NodeStatus::SUCCESS;
+}
+
+void from_img_point_to_pan_tilt_c(double _x, double _y, const sensor_msgs::CameraInfo& _camera_info, double& _pan, double& _tilt)
+{
+ int col = (int)_x;
+ int row = (int)_y;
+ if (col < 0)
+ {
+ ROS_WARN_STREAM("head_helpers::from_img_point_to_pan_tilt-> x<0: set it to 0");
+ col = 0;
+ }
+ if (col > _camera_info.width-1)
+ {
+ ROS_WARN_STREAM("head_helpers::from_img_point_to_pan_tilt-> x>" << (int)_camera_info.width-1 << ". set it to " << (int)_camera_info.width-1);
+ col = _camera_info.width-1;
+ }
+
+ if (row < 0)
+ {
+ ROS_WARN_STREAM("head_helpers::from_img_point_to_pan_tilt-> y<0: set it to 0");
+ row = 0;
+ }
+ if (row > _camera_info.height-1)
+ {
+ ROS_WARN_STREAM("head_helpers::from_img_point_to_pan_tilt-> y>" << (int)_camera_info.height-1 << ". set it to " << (int)_camera_info.height-1);
+ row = _camera_info.height-1;
+ }
+ _pan = std::atan2(col-_camera_info.K[2],_camera_info.K[0])*(invert_pan? -1: 1);
+ _tilt = std::atan2(row-_camera_info.K[5],_camera_info.K[4])*(invert_tilt? -1: 1);
+}
+
+BT::NodeStatus from_world_point_to_pan_tilt(BT::TreeNode& _self)
+{
+ BT::Optional<geometry_msgs::PoseStamped> pose_p = _self.getInput<geometry_msgs::PoseStamped>("pose");
+ BT::Optional<std::string> follower_frame_p = _self.getInput<std::string>("follower_frame");
+ geometry_msgs::PoseStamped pose;
+ std::string follower_frame;
+ double pan;
+ double tilt;
+ _self.setOutput("pan", pan);
+ _self.setOutput("tilt", tilt);
+ if (!pose_p || !follower_frame_p)
+ {
+ ROS_ERROR_STREAM("head_helpers::from_world_point_to_pan_tilt-> Incorrect or missing input. It needs the following input ports:"
+ << " pose (geometry_msgs::PoseStamped) and follower_frame (std::string)");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ pose = pose_p.value();
+ follower_frame = follower_frame_p.value();
+
+ if (!from_world_point_to_pan_tilt_c(pose, follower_frame, pan, tilt))
+ return BT::NodeStatus::FAILURE;
+ _self.setOutput("pan", pan);
+ _self.setOutput("tilt", tilt);
+ return BT::NodeStatus::SUCCESS;
+}
+
+bool from_world_point_to_pan_tilt_c(const geometry_msgs::PoseStamped& _pose, const std::string& _follower_frame, double& _pan, double& _tilt)
+{
+ geometry_msgs::TransformStamped tf_track_point_frame_msg;
+ geometry_msgs::PoseStamped point_in_follower_frame;
+
+ try{
+ if(tf2_buffer_p->canTransform(_follower_frame, _pose.header.frame_id, _pose.header.stamp, ros::Duration(1.0)))
+ {
+ tf_track_point_frame_msg = tf2_buffer_p->lookupTransform(_follower_frame, _pose.header.frame_id, _pose.header.stamp);
+ tf2::doTransform(_pose,point_in_follower_frame,tf_track_point_frame_msg);
+
+ _pan = std::atan2(point_in_follower_frame.pose.position.x, point_in_follower_frame.pose.position.z)*(invert_pan? -1: 1);
+ _tilt = std::atan2(point_in_follower_frame.pose.position.y, point_in_follower_frame.pose.position.z)*(invert_tilt? -1: 1);
+ // ROS_INFO_STREAM("from_world_point_to_pan_tilt_c -> after tf frame " << point_in_follower_frame.header.frame_id);
+ // ROS_INFO_STREAM("from_world_point_to_pan_tilt_c -> after tf " << point_in_follower_frame.pose);
+ // ROS_INFO_STREAM("from_world_point_to_pan_tilt_c -> pan " << _pan);
+ // ROS_INFO_STREAM("from_world_point_to_pan_tilt_c -> tilt " << _tilt);
+ return true;
+ }
+ else
+ {
+ ROS_WARN_STREAM("head_helpers::from_world_point_to_pan_tilt_c-> No transform found from " << _follower_frame << " to " << _pose.header.frame_id << " at " << _pose.header.stamp);
+ return false;
+ }
+ }catch(tf2::TransformException &ex){
+ ROS_ERROR_STREAM("head_helpers::from_world_point_to_pan_tilt_c-> TF2 Exception: " << ex.what());
+ return false;
+ }
+}
\ No newline at end of file
diff --git a/src/xml/bt_definitions.xml b/src/xml/bt_definitions.xml
index 35e3ec719e87f19a363fc0ee85e94fcf76ee97ba..c96154b3ce68cd176dd1b8cbe80ab577e48a8505 100644
--- a/src/xml/bt_definitions.xml
+++ b/src/xml/bt_definitions.xml
@@ -36,6 +36,53 @@
<input_port name="msg"> (std::string): Msg to print.</input_port>
<input_port name="type"> (std::string): It must be info, warn, error or debug.</input_port>
</Action>
+ <Action ID="get_config">
+ <output_port name="dyn_config"> (ConfigClass *): Pointer to config class.</output_port>
+ </Action>
+ <Action ID="dyn_reconf_get_string">
+ <input_port name="dyn_config"> (ConfigClass *): Pointer to config class.</input_port>
+ <input_port name="param_name"> (std::string): Parameter name.</input_port>
+ <output_port name="param_value"> (std::string): Parameter value.</output_port>
+ </Action>
+ <Action ID="dyn_reconf_get_integer">
+ <input_port name="dyn_config"> (ConfigClass *): Pointer to config class.</input_port>
+ <input_port name="param_name"> (std::string): Parameter name.</input_port>
+ <output_port name="param_value"> (Int): Parameter value.</output_port>
+ </Action>
+ <Action ID="dyn_reconf_get_unsigned_integer">
+ <input_port name="dyn_config"> (ConfigClass *): Pointer to config class.</input_port>
+ <input_port name="param_name"> (std::string): Parameter name.</input_port>
+ <output_port name="param_value"> (unsigned int): Parameter value.</output_port>
+ </Action>
+ <Action ID="dyn_reconf_get_double">
+ <input_port name="dyn_config"> (ConfigClass *): Pointer to config class.</input_port>
+ <input_port name="param_name"> (std::string): Parameter name.</input_port>
+ <output_port name="param_value"> (double): Parameter value.</output_port>
+ </Action>
+ <Action ID="dyn_reconf_get_double_vector">
+ <input_port name="dyn_config"> (ConfigClass *): Pointer to config class.</input_port>
+ <input_port name="param_name"> (std::string): Parameter name.</input_port>
+ <output_port name="param_value"> (std::vector double): Parameter value.</output_port>
+ </Action>
+ <Action ID="get_bool">
+ <input_port name="dyn_config"> (ConfigClass *): Pointer to config class.</input_port>
+ <input_port name="param_name"> (std::string): Parameter name.</input_port>
+ <output_port name="param_value"> (bool): Parameter value.</output_port>
+ </Action>
+ <!-- Head helepers -->
+ <Action ID="from_img_point_to_pan_tilt">
+ <input_port name="x"> Image x coordinate</input_port>
+ <input_port name="y"> Image y coordinate</input_port>
+ <input_port name="camera_info"> Camera info of the camera</input_port>
+ <output_port name="pan"> Pan angle to center the image on the target point</output_port>
+ <output_port name="tilt"> Tilt angle to center the image on the target point</output_port>
+ </Action>
+ <Action ID="from_world_point_to_pan_tilt">
+ <input_port name="pose"> Target world point</input_port>
+ <input_port name="follower_frame"> Frame that must follow the target point</input_port>
+ <output_port name="pan"> Pan angle to center the image on the target point</output_port>
+ <output_port name="tilt"> Tilt angle to center the image on the target point</output_port>
+ </Action>
</TreeNodesModel>
<!-- ////////// -->
-</root>
\ No newline at end of file
+</root>