Updated to iri_ros_tools last changes. Updated BT interface to match with tiago_head_module

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})

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.
       *

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.
       *

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 -->

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);

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();

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");