diff --git a/README.md b/README.md new file mode 100644 index 0000000000000000000000000000000000000000..0532adc131cda28b0444e8a46591093e5001d559 --- /dev/null +++ b/README.md @@ -0,0 +1,159 @@ +# Description + +IRI module to control a chain of servos. It has three module interfaces: PointTo, PointToTracker and MoveToAngles. + +# Module Interface + +### PointTo + +It is a wrapper of a [control_msgs/PointHeadAction](http://docs.ros.org/api/control_msgs/html/action/PointHead.html) client. + An example of [control_msgs/PointHeadAction](http://docs.ros.org/api/control_msgs/html/action/PointHead.html) server + can be found on [iri_point_to](https://gitlab.iri.upc.edu/labrobotica/ros/devices/joints/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](https://gitlab.iri.upc.edu/labrobotica/ros/devices/joints/iri_joints_msgs/-/blob/master/action/pointHeadTracker.action) client. + It updates the target information publisihng a [control_msgs/PointHeadActionGoal](http://docs.ros.org/melodic/api/control_msgs/html/action/PointHead.html). + An example of a [control_msgs/PointHeadActionGoal](http://docs.ros.org/melodic/api/control_msgs/html/action/PointHead.html) server can be + found on [iri_point_to](https://gitlab.iri.upc.edu/labrobotica/ros/devices/joints/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](http://docs.ros.org/api/control_msgs/html/action/FollowJointTrajectory.html) client. + It has the following API: + +- *bool move_to_angles(std::vector<std::string> joint_names, std::vector<double> angles, std::vector<double> 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](https://gitlab.iri.upc.edu/labrobotica/ros/devices/joints/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. + +# ROS Interface + +### Subscribed topics + +- */joint_states* ([sensor_msgs/JointState](http://docs.ros.org/melodic/api/sensor_msgs/html/msg/JointState.html)): + 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 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. + +##### PointToTracker + +The usual ActionModule parameters. + +##### MoveToAngles + +The usual ActionModule parameters and: + +- **move_to_angles_time_from_start** *(Integer; Default: 10; Max: 100; Min: 0)*: + Max time in seconds to finish the action. + +##### JointStates + +The usual WatchDog parameters. + +# Dependencies + +This package has the following ROS dependencies: + + * [control_msgs/PointHeadAction](http://docs.ros.org/api/control_msgs/html/action/PointHead.html) + * [sensor_msgs/JointState](http://docs.ros.org/melodic/api/sensor_msgs/html/msg/JointState.html) + * [control_msgs/FollowJointTrajectoryAction](http://docs.ros.org/api/control_msgs/html/action/FollowJointTrajectory.html) + * [actionlib](http://wiki.ros.org/actionlib) + * [roscpp](http://wiki.ros.org/roscpp) + * [dynamic_reconfigure](http://wiki.ros.org/dynamic_reconfigure) + +This dependencies can be installed with the following command: + +``` +sudo apt install ros-kinetic-control-msgs ros-kinetic-sensor-msgs ros-kinetic-actionlib ros-kinetic-roscpp ros-kinetic-dynamic-reconfigure +``` + +It also has the following external dependencies: + * [behaviortree_cpp_v3](https://github.com/BehaviorTree/BehaviorTree.CPP) + +This node has the following ROS IRI dependencies: + + * [iri_base_algorithm](https://gitlab.iri.upc.edu/labrobotica/ros/iri_core/iri_base_algorithm/tree/kinetic_migration) + * [iri_ros_tools](https://gitlab.iri.upc.edu/labrobotica/ros/iri_core/iri_ros_tools) + * [iri_behaviortree](https://gitlab.iri.upc.edu/labrobotica/ros/devices/joints/iri_behaviortree) + * [iri_joints_msgs](https://gitlab.iri.upc.edu/labrobotica/ros/devices/joints/iri_joints_msgs) + +It also has the following IRI libraries dependencies: + + * [iriutils](https://gitlab.iri.upc.edu/labrobotica/algorithms/iriutils) + +# 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 provide two ROS node module client examples and the corresponding launch files for each example.