## For instruction on writing tutorials
## http://www.ros.org/wiki/WritingTutorials
####################################
##FILL ME IN
####################################
## for a custom note with links:
## note =
## for the canned note of "This tutorial assumes that you have completed the previous tutorials:" just add the links 
## note.0= 
## descriptive title for the tutorial
## title = Best-practice for joint control access
## multi-line description to be displayed in search 
## description = How to access the joint controllers the best way
## the next tutorial description (optional)
## next =
## links to next tutorial (optional)
## next.0.link=
## next.1.link=
## what level user is this tutorial for 
## level= (BeginnerCategory, IntermediateCategory, AdvancedCategory)
## keywords = pr2_controllers, joint, control
####################################

<<IncludeCSTemplate(TutorialCSHeaderTemplate)>>

<<TableOfContents(4)>>

## AUTOGENERATED DO NOT DELETE 
## TutorialCategory
## FILL IN THE STACK TUTORIAL CATEGORY HERE

=== Introduction ===

This tutorial provides a best-practice solution to access and control joints in the shadow hand through controllers/command topics. Meanwhile this will help you remove any use of the deprecated /sendupdate topics

We suppose that you have installed ROS and the shadow hand stack. Works in simulation and on the real hand.

=== Context ===

The same way as in the PR2, the Shadow etherCAT hand uses now host-side controllers sending torque demands to the low-level controllers inside the hand. 
For this reason, several different controllers can exist and can access the joints. Each controller has a different topic to send its command to. 

For instance : 
Finger proximal joint FFJ3 could be controller by 
 * a velocity controller which awaits the command on  sh_ffj3_velocity_controller/command
 * a mixed controller which awaits the command on sh_ffj3_mixed_position_velocity_controller/command

The main question arises :
 '''How can my program know what topic to use to control a joint ?'''


=== Concept ===

The answer to the question is : '''controller_manager'''

The controller manager  manages (loads, unloads, switch, start, stops) the controllers of a robot for each of its joints. Several controllers can be loaded at the same time for a same joint, but only one can be running at the same time.

The manager also offers a service (ListControllers) for your external software to know about the controllers and their status. This service is the main part of the solution proposed.

Indeed, by querying the running controllers, your software can directly know which topics it should publish to. But one thing is missing. The controller manager will not give you the association between a controller name and the joint it controls.

We will rely on the parameter server to query that association from the controller directly.

=== Solution ===

This part of code should be done in your initialization function to create the mapping
===== Cpp =====
{{{ 
#!cplusplus 
ros::NodeHandle nodehandler;
std::map<std::string,std::string> jointControllerMap; 

ros::service::waitForService("pr2_controller_manager/list_controllers");
ros::ServiceClient controller_list_client = nodehandler.serviceClient<pr2_mechanism_msgs::ListControllers>("pr2_controller_manager/list_controllers");


pr2_mechanism_msgs::ListControllers controller_list;
std::string controlled_joint_name;

controller_list_client.call(controller_list);

for (unsigned int i=0;i<controller_list.response.controllers.size() ;i++ )
{
  if(controller_list.response.state[i]=="running")
  {
    if (nodehandler.getParam("/"+controller_list.response.controllers[i]+"/joint", controlled_joint_name))
    {
      ROS_DEBUG("controller %d:%s controls joint %s\n",i,controller_list.response.controllers[i].c_str(),controlled_joint_name.c_str());
      jointControllerMap[controlled_joint_name]= controller_list.response.controllers[i] ;
    }
   }
  }
}}}

Then, you can use your map to send data to the correct controller just by using the joint name
{{{
#!cplusplus 
std::string controller_name=jointControllerMap["FFJ3"];
}}}

Basically add /command to the controller name and voilà, you know the currently running controller's topic name for your joint.

===== Cpp code detailed =====
{{{ 
#!cplusplus 
ros::NodeHandle nodehandler;
std::map<std::string,std::string> jointControllerMap; 
}}}
Prepare a mapping table between controller name and joint name

{{{
#!cplusplus 
ros::service::waitForService("pr2_controller_manager/list_controllers");
ros::ServiceClient controller_list_client = nodehandler.serviceClient<pr2_mechanism_msgs::ListControllers>("pr2_controller_manager/list_controllers");
}}}
wait for controller manager service to be ready and prepare a service client to access the ListControllers service.

The next part will find the association between controller and joint names. It actually inits jointControllerMapping
{{{
#!cplusplus 
pr2_mechanism_msgs::ListControllers controller_list;
std::string controlled_joint_name;

controller_list_client.call(controller_list);
}}}
Query the controller for the controllers list
{{{
#!cplusplus 
for (unsigned int i=0;i<controller_list.response.controllers.size() ;i++ )
{
  if(controller_list.response.state[i]=="running")
  {

}}}
For each controller name in the retrieved list, if this controller is running ...

{{{
#!cplusplus 
    if (nodehandler.getParam("/"+controller_list.response.controllers[i]+"/joint", controlled_joint_name))
    {
      jointControllerMap[controlled_joint_name]= controller_list.response.controllers[i] ;
    }
   }
  }
}}}
check the param server for the controlled joint name, if it exists, then add the controller name and the joint name to the mapping table.

===== Python =====

TO BE COMPLETED