Commit 63aadbc3 authored by Alejandro Lopez Gestoso's avatar Alejandro Lopez Gestoso
Browse files

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

parent f35f7163
......@@ -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})
......
......@@ -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.
*
......
......@@ -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.
*
......
......@@ -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 -->
......
......@@ -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);
......
......@@ -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();
......
......@@ -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");
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment