diff --git a/CMakeLists.txt b/CMakeLists.txt
index ea2e102393b817affe0e82f9d092cf9d169b09c5..8a3ab4d01ad608327772e3a860b73ecb351ba4f2 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -7,7 +7,7 @@ add_definitions(-std=c++11)
## Find catkin macros and libraries
## 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 iri_ros_tools iri_base_algorithm control_msgs sensor_msgs iri_joints_msgs actionlib behaviortree_cpp_v3 iri_behaviortree)
+find_package(catkin REQUIRED COMPONENTS roscpp dynamic_reconfigure iri_ros_tools iri_base_algorithm control_msgs sensor_msgs geometry_msgs iri_joints_msgs actionlib behaviortree_cpp_v3 iri_behaviortree)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
@@ -103,7 +103,7 @@ generate_dynamic_reconfigure_options(
catkin_package(
INCLUDE_DIRS include
LIBRARIES iri_joints_module iri_joints_module_bt
- CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools iri_base_algorithm control_msgs sensor_msgs iri_joints_msgs actionlib behaviortree_cpp_v3 iri_behaviortree
+ CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools iri_base_algorithm control_msgs sensor_msgs geometry_msgs iri_joints_msgs actionlib behaviortree_cpp_v3 iri_behaviortree
# DEPENDS system_lib
)
@@ -138,6 +138,7 @@ target_link_libraries(iri_joints_module_bt ${iriutils_LIBRARY})
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
add_dependencies(iri_joints_module control_msgs_generate_messages_cpp)
+add_dependencies(iri_joints_module geometry_msgs_generate_messages_cpp)
add_dependencies(iri_joints_module ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(iri_joints_module_bt ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
diff --git a/include/iri_joints_module/iri_joints_module.h b/include/iri_joints_module/iri_joints_module.h
index 3172123719a0de9d4d5899167fb9fe2ea2d4d134..ee08ad977a3f09cf5900d1077456edaf987f379a 100644
--- a/include/iri_joints_module/iri_joints_module.h
+++ b/include/iri_joints_module/iri_joints_module.h
@@ -15,6 +15,7 @@
#include <control_msgs/PointHeadActionGoal.h>
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <sensor_msgs/JointState.h>
+#include <geometry_msgs/PointStamped.h>
#include <iri_joints_msgs/pointHeadTrackerAction.h>
// Dynamic reconfigure header
@@ -84,7 +85,7 @@ class CIriJointsModule : public CModule<iri_joints_module::IriJointsModuleConfig
* \brief
*
*/
- CModuleAction<control_msgs::PointHeadAction> point_to_action;
+ CModuleAction<control_msgs::PointHeadAction, iri_joints_module::IriJointsModuleConfig> point_to_action;
/**
* \brief
*
@@ -102,7 +103,7 @@ class CIriJointsModule : public CModule<iri_joints_module::IriJointsModuleConfig
* \brief
*
*/
- CModuleDynReconf point_to_reconf;
+ CModuleDynReconf<iri_joints_module::IriJointsModuleConfig> point_to_reconf;
// JointStates
/**
@@ -126,7 +127,7 @@ class CIriJointsModule : public CModule<iri_joints_module::IriJointsModuleConfig
* \brief
*
*/
- CModuleAction<iri_joints_msgs::pointHeadTrackerAction> track_action;
+ CModuleAction<iri_joints_msgs::pointHeadTrackerAction, iri_joints_module::IriJointsModuleConfig> track_action;
/**
* \brief
*
@@ -140,7 +141,7 @@ class CIriJointsModule : public CModule<iri_joints_module::IriJointsModuleConfig
//FollowJointTrayectoryAction
- CModuleAction<control_msgs::FollowJointTrajectoryAction> move_to_angle_action;
+ CModuleAction<control_msgs::FollowJointTrajectoryAction, iri_joints_module::IriJointsModuleConfig> move_to_angle_action;
control_msgs::FollowJointTrajectoryGoal move_to_angle_goal;
bool new_angles;
@@ -222,6 +223,15 @@ class CIriJointsModule : public CModule<iri_joints_module::IriJointsModuleConfig
*/
bool target_update(double x, double y, double z);
+ /**
+ * \brief Update the target at x,y,z position.
+ *
+ * \param point cartesian position to point to, with a valid reference frame id.
+ *
+ * \return True if action is called.
+ */
+ bool target_update(geometry_msgs::PointStamped &point);
+
/**
* \brief Function to start the tracker action.
*
diff --git a/include/iri_joints_module/iri_joints_module_bt.h b/include/iri_joints_module/iri_joints_module_bt.h
index f36fb635c5fcc58ba7a23e98e3298881ca810368..3fab928b600a8f64d0d8c2836efa89a31f4fdafb 100644
--- a/include/iri_joints_module/iri_joints_module_bt.h
+++ b/include/iri_joints_module/iri_joints_module_bt.h
@@ -40,6 +40,14 @@ class CIriJointsModuleBT
*/
CIriJointsModuleBT();
+ /**
+ * \brief Constructor with module name.
+ *
+ * \param module_name The module name.
+ *
+ */
+ CIriJointsModuleBT(std::string &module_name);
+
/**
* \brief Register all conditions and actions needed for using the module
*
@@ -273,6 +281,28 @@ class CIriJointsModuleBT
*/
BT::NodeStatus target_update(BT::TreeNode& self);
+ /**
+ * \brief Synchronized target update function function.
+ *
+ * This function calls target_update of iri_joints_module.
+ *
+ * It has the following input ports:
+ *
+ * point (geometry_msgs::PointStamped): The target Point.
+ *
+ * It also has a bidirectional port:
+ *
+ * new_goal (bool): If it's a new_goal.
+ *
+ * \param self node with the required inputs defined as follows:
+ *
+ * \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 target_update_pointstamped(BT::TreeNode& self);
+
/**
* \brief Synchronized enable tracker function.
*
diff --git a/package.xml b/package.xml
index 710197b544d7dcf7f7f415e09d8fd7d28d1db525..edbfa25880e49f033734109f55a69e74bc5e042c 100644
--- a/package.xml
+++ b/package.xml
@@ -50,6 +50,7 @@
<build_depend>control_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>iri_joints_msgs</build_depend>
+ <build_depend>geometry_msgs</build_depend>
<!-- new dependencies -->
<!--<build_depend>new build dependency</build_depend>-->
@@ -63,6 +64,7 @@
<run_depend>control_msgs</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>iri_joints_msgs</run_depend>
+ <run_depend>geometry_msgs</run_depend>
<!-- new dependencies -->
diff --git a/src/iri_joints_client_bt_alg_node.cpp b/src/iri_joints_client_bt_alg_node.cpp
index 6a6aa55b136c820dfd1558d33a305b776a352a63..db83ffc65c7710c9bae5a328cdef81290eca0169 100644
--- a/src/iri_joints_client_bt_alg_node.cpp
+++ b/src/iri_joints_client_bt_alg_node.cpp
@@ -47,7 +47,7 @@ IriJointsClientBtAlgNode::IriJointsClientBtAlgNode(void) :
#ifdef ZMQ_FOUND
this->publisher_zmq = new BT::PublisherZMQ(this->tree);
#endif
- BT::printTreeRecursively(this->tree.root_node);
+ // BT::printTreeRecursively(this->tree.root_node);
}
IriJointsClientBtAlgNode::~IriJointsClientBtAlgNode(void)
@@ -84,7 +84,7 @@ void IriJointsClientBtAlgNode::mainNodeThread(void)
if (send_ticks && running)
{
ROS_INFO("---Tick---");
- this->status = this->tree.root_node->executeTick();
+ this->status = this->tree.tickRoot();
if (this->status != BT::NodeStatus::RUNNING)
running = false;
ROS_INFO_STREAM("Status: " << this->status);
diff --git a/src/iri_joints_module.cpp b/src/iri_joints_module.cpp
index 4a900d7fc1c470bd70bfd0beca95cb9942cda896..9876ee1ba5e2e11e8f3461e21cdac5d61550b99e 100644
--- a/src/iri_joints_module.cpp
+++ b/src/iri_joints_module.cpp
@@ -22,7 +22,13 @@ CIriJointsModule::CIriJointsModule(const std::string &name,const std::string &na
this->enable_tracker_pending = false;
// initialize JointStates subscriber
- this->joint_states_watchdog.reset(ros::Duration(this->config.joint_states_watchdog_time_s));
+ if(!this->module_nh.getParam("joint_states_watchdog_time_s", this->config.joint_states_watchdog_time_s))
+ {
+ ROS_WARN("CTiagoNavModule::CTiagoNavModule: param 'joint_states_watchdog_time_s' not found. Configuring wathcdog with 1.0 sec");
+ this->joint_states_watchdog.reset(ros::Duration(1.0));
+ }
+ else
+ this->joint_states_watchdog.reset(ros::Duration(this->config.joint_states_watchdog_time_s));
this->joint_states_subscriber = this->module_nh.subscribe("joint_states",1,&CIriJointsModule::joint_states_callback,this);
}
@@ -485,6 +491,35 @@ bool CIriJointsModule::target_update(double x, double y, double z)
return true;
}
+bool CIriJointsModule::target_update(geometry_msgs::PointStamped &point)
+{
+ if (!track_action.is_enabled())
+ return true;
+ this->lock();
+ if (this->state == IRI_JOINTS_MODULE_POINT_WAIT)
+ {
+ ROS_WARN("CIriJointsModule::target_update-> PointTo action is active. Dropping this goal.");
+ this->unlock();
+ return false;
+ }
+ else if (this->state == IRI_JOINTS_MODULE_MOVE_ANGLES)
+ {
+ ROS_WARN("CIriJointsModule::target_update-> MoveToAngles is active. Dropping this goal.");
+ this->unlock();
+ return false;
+ }
+ else if (this->state == IRI_JOINTS_MODULE_IDLE)
+ {
+ ROS_WARN("CIriJointsModule::target_update-> Tracking is not active. Dropping this goal.");
+ this->unlock();
+ return false;
+ }
+ this->target_goal.goal.target = point;
+ this->new_point_goal = true;
+ this->unlock();
+ return true;
+}
+
bool CIriJointsModule::enable_tracker(void)
{
this->lock();
diff --git a/src/iri_joints_module_bt.cpp b/src/iri_joints_module_bt.cpp
index 7fbec8afd6aff45dd980a79190ff96a9c574018c..fc2e07c59da34909b7e9cb475ac69f147d6bd70b 100644
--- a/src/iri_joints_module_bt.cpp
+++ b/src/iri_joints_module_bt.cpp
@@ -8,6 +8,12 @@ CIriJointsModuleBT::CIriJointsModuleBT() :
}
+CIriJointsModuleBT::CIriJointsModuleBT(std::string &module_name) :
+ joints_module(module_name,ros::this_node::getName())
+{
+
+}
+
void CIriJointsModuleBT::init(IriBehaviorTreeFactory &factory)
{
//Definition of ports
@@ -23,6 +29,8 @@ void CIriJointsModuleBT::init(IriBehaviorTreeFactory &factory)
BT::PortsList set_base_frame_ports = {BT::InputPort<std::string>("base_frame")};
BT::PortsList set_urdf_param_ports = {BT::InputPort<std::string>("urdf_param")};
+ BT::PortsList target_update_point_port = {BT::InputPort<geometry_msgs::PointStamped>("point"), BT::BidirectionalPort<bool>("new_goal")};
+
//Registration of conditions
factory.registerSimpleCondition("is_point_to_finished", std::bind(&CIriJointsModuleBT::is_point_to_finished, this));
factory.registerSimpleCondition("is_move_to_angles_finished", std::bind(&CIriJointsModuleBT::is_move_to_angles_finished, this));
@@ -60,6 +68,7 @@ void CIriJointsModuleBT::init(IriBehaviorTreeFactory &factory)
factory.registerSimpleAction("set_point_to_inv_kin_solver_tolerance", std::bind(&CIriJointsModuleBT::set_tolerance, this, std::placeholders::_1), set_tolerance_ports);
factory.registerSimpleAction("set_point_to_kin_chain_base_frame", std::bind(&CIriJointsModuleBT::set_base_frame, this, std::placeholders::_1), set_base_frame_ports);
factory.registerSimpleAction("set_point_to_kin_chain_urdf_param", std::bind(&CIriJointsModuleBT::set_urdf_param, this, std::placeholders::_1), set_urdf_param_ports);
+ factory.registerSimpleAction("point_to_tracker_target_update_pointstamped", std::bind(&CIriJointsModuleBT::target_update_pointstamped, this, std::placeholders::_1), target_update_point_port);
}
CIriJointsModuleBT::~CIriJointsModuleBT(void)
@@ -251,6 +260,35 @@ BT::NodeStatus CIriJointsModuleBT::target_update(BT::TreeNode& self)
return BT::NodeStatus::SUCCESS;
}
+BT::NodeStatus CIriJointsModuleBT::target_update_pointstamped(BT::TreeNode& self)
+{
+ ROS_DEBUG("CIriJointsModuleBT::target_update_pointstamped-> target_update");
+ BT::Optional<geometry_msgs::PointStamped> point = self.getInput<geometry_msgs::PointStamped>("point");
+ BT::Optional<bool> new_goal_in = self.getInput<bool>("new_goal");
+
+ bool new_goal;
+ if (!new_goal_in)
+ new_goal = false;
+ else
+ new_goal = new_goal_in.value();
+
+ if (new_goal)
+ {
+ if (!point)
+ {
+ ROS_ERROR("CIriJointsModuleBT::target_update_pointstamped-> Incorrect or missing input. It needs the following input ports: point(geometry_msgs::PointStamped)");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ geometry_msgs::PointStamped point_value = point.value();
+
+ this->joints_module.target_update(point_value);
+ }
+ if (new_goal)
+ self.setOutput("new_goal", false);
+ return BT::NodeStatus::SUCCESS;
+}
+
BT::NodeStatus CIriJointsModuleBT::enable_tracker(void)
{
ROS_DEBUG("CIriJointsModuleBT::enable_tracker-> enable_tracker");