Description

IRI module to control a chain of servos. It has three module interfaces: PointTo, PointToTracker and MoveToAngles. Here more useful information about this module can be found.

Module Interface

PointTo

It is a wrapper of a control_msgs/PointHeadAction client. An example of control_msgs/PointHeadAction server can be found on iri_point_to. It has the following API:

• bool point_to(double x, double y, double z, double max_vel): To start the action.

• void cancel_point_to(void): To cancel the action.

• bool is_point_to_finished(void): To know if the action has finished.

This Interface can return any of the typical module status.

PointToTracker

It is a wrapper of a iri_joints_msgs/pointHeadTrackerAction client. It updates the target information publisihng a control_msgs/PointHeadActionGoal. An example of a control_msgs/PointHeadActionGoal server can be found on iri_point_to. It has the following API:

• bool enable_tracker(void): To start the action.

• void cancel_tracker(void): To cancel the action.

• bool is_tracker_finished(void): To know if the action has finished.

• bool target_update(double x, double y, double z): To update the target information.

This Interface can return any of the typical module status and target_lost status.

MoveToAngles

It is a wrapper of a control_msgs/FollowJointTrajectoryAction client. It has the following API:

• bool move_to_angles(std::vector<std::string> joint_names, std::vector angles, std::vector vel): To start the action.

• void cancel_move_to_angles(void): To cancel the action.

• bool is_move_to_angles_finished(void): To know if the action has finished.

This Interface can return any of the typical module status.

Configuration interface

You can configure any Dynamic Reconfigure parameter of iri_point_to with set_parameter(parameter_name, parameter_value) function. It also has the following explicits functions:

• bool set_tolerance(double tolerance): To change the inverse kinematics solver tolerance.

• bool set_base_frame(std::string &base_frame) To change the kinematic chain base frame.

• bool set_urdf_param(std::string &urdf_param): To change the ROS param name to get the urdf.

BT Interface

The BehaviorTree layer provides a function called init to register the following Actions and Conditions:

Common

It provides a Simple Condition for each module status. The name of each Simple Condition is is_joints_module_<status>.

Point_to

• sync_point_to: Simple Action to start the action.

  • Input ports: x (double), y (double), z (double) and max_vel (double)

• async_point_to: Iri Async Action to start the action.

  • Input ports: x (double), y (double), z (double) and max_vel (double)
  • Bidirectional ports: new_goal (bool)

• cancel_point_to: Simple Action to cancel the action.

• is_point_to_finished: Simple Condition to know if the action has finished.

• async_is_point_to_finished: Iri Async Action to know if the action has finished.

PointToTracker

• enable_point_to_tracker: Simple Action to start the action.

• point_to_tracker_update_target: Simple Action to update the target.

  • Input ports: x (double), y (double) and z (double)

• cancel_point_to_tracker: Simple Action to cancel the action.

• is_point_to_tracker_finished: Simple Condition to know if the action has finished.

• async_is_point_to_tracker_finished: Iri Async Action to know if the action has finished.

MoveToAngles

• sync_move_to_angles: Simple Action to start the action.

  • Input ports: joint_names (std::vector<std::string>), angles (std::vector<std::string>) and vel (std::vector<std::string>)

• async_move_to_angles: Iri Async Action to start the action.

  • Input ports: joint_names (std::vector<std::string>), angles (std::vector<std::string>) and vel (std::vector<std::string>)
  • Bidirectional ports:new_goal (bool)

• cancel_move_to_angles: Simple Action to cancel the action.

• is_move_to_angles_finished: Simple Condition to know if the action has finished.

• async_is_move_to_angles_finished: Iri Async Action to know if the action has finished.

Configuration

• set_point_to_inv_kin_solver_tolerance: Simple Action to set point_to inverse kinematic solver tolerance.

  • Input ports: tol (double)

• set_point_to_kin_chain_base_frame: Simple Action to set point_to kinematic chain base frame.

  • Input ports: base_frame (std::string)

• set_point_to_kin_chain_urdf_param: Simple Action to set the ROS URDF parameter.

  • Input ports: urdf_param (std::string)

ROS Interface

Subscribed topics

• ~/joint_states (sensor_msgs/JointState): The servos' current postions. If no joint_states is received the module return a no_joint_states status.

Parameters

• ~/frame_id (String; Default:joints_module): The frame ID.

• ~/rate_hz (Double; Default: 1.0; Max: 100.0; Min: 1.0): Joints module rate in Hz.

PointTo

The usual ActionModule parameters named as ~/pt_<ActionModule_parameter> and:

• ~/pointing_frame (String; Default:): The pointing frame.

• ~/pointing_axis (Integer; Default: 2; Max: 2; Min: 0): The pointing axis (X=0, Y=1, Z=2).

• ~/point_frame (String; Default:): The point's frame.

• ~/pt_enabled (Bool; Default: True): Enable pointTo action execution.

PointToTracker

The usual ActionModule parameters named as ~/tracker_<ActionModule_parameter> and:

• ~/tracker_enabled (Bool; Default: True): Enable pointTo tracker action execution.

MoveToAngles

The usual ActionModule parameters named as ~/move_to_angles_<ActionModule_parameter> and:

• ~/move_to_angles_time_from_start (Integer; Default: 10; Max: 100; Min: 0): Max time in seconds to finish the action.

• ~/move_to_angles_pos_tol (Double; Default: 0.02; Min: -1.0): Path and goal position tolerance in rad or m.

• ~/move_to_angles_vel_tol (Double; Default: 0.0; Min: -1.0): Path and goal velocity tolerance in rad/s or m/s.

• ~/move_to_angles_acc_tol (Double; Default: 0.0; Min: -1.0): Path and goal acceleration tolerance in rad/s² or m/s².

• ~/move_to_angles_time_tol (Double; Default: 0.1; Max: 1.0; Min: 0.0): Path and goal time tolerance in percentaje.

• ~/move_to_angles_enabled (Bool; Default: True): Enable move to angles action execution

JointStates

The usual WatchDog parameters named as ~/joint_states_<WatchDog_parameter>.

Dependencies

This package has the following ROS dependencies:

• control_msgs/PointHeadAction
• sensor_msgs/JointState
• control_msgs/FollowJointTrajectoryAction
• actionlib
• roscpp
• dynamic_reconfigure

This dependencies can be installed with the following command:

sudo apt install ros-$ROS_DISTRO-control-msgs ros-$ROS_DISTRO-sensor-msgs ros-$ROS_DISTRO-actionlib ros-$ROS_DISTRO-roscpp ros-$ROS_DISTRO-dynamic-reconfigure

It also has the following external dependencies:

• behaviortree_cpp_v3

This node has the following ROS IRI dependencies:

• iri_base_algorithm
• iri_ros_tools
• iri_behaviortree
• iri_joints_msgs

Follow the previous links for instructions on how to install the required ROS IRI dependencies.

It also has the following IRI libraries dependencies:

• iriutils

Follow the previous links for instructions on how to install the required IRI dependencies.

Installation

• Move to workspace:

roscd && cd ../src

• Clone the repository:

git clone https://gitlab.iri.upc.edu/labrobotica/ros/devices/joints/iri_joints_module.git

• Compile:

roscd && cd .. && catkin_make -DCMAKE_BUILD_TYPE=Release

How to use it

This package provides a ROS node module client example for the module interface and the BehavoirTree interface. Both examples uses a default module configuration file located on config folder.

With the module client example all module functionalities can be tested through the Dynamic Reconfigure. To run it execute the following command:

roslaunch iri_joints_module joints_client_example.launch

With the module BT client example you can run an example tree for each module interface. The tree examples are located on src/xml folder. To run it execute the following command:

roslaunch iri_joints_module joints_client_bt_example.launch