mia_hand_description  rel 1.0.0
remap_mia_joint_states.cpp
Go to the documentation of this file.
1 #include "ros/ros.h"
2 #include "std_msgs/String.h"
3 #include "sensor_msgs/JointState.h"
4 #include <iostream>
5 #include <string>
6 #include <cmath>
7 #include <urdf/model.h>
8 #include <joint_limits_interface/joint_limits.h>
9 #include <joint_limits_interface/joint_limits_interface.h>
10 #include <joint_limits_interface/joint_limits_rosparam.h>
11 #include <joint_limits_interface/joint_limits_urdf.h>
12 
14 
15 
19 class myRemap
20 {
21 
22  public:
23 
28  {
29  pub = n.advertise<sensor_msgs::JointState>("miaviz_joint_states", 1000);
30  sub = n.subscribe("joint_states", 1000, &myRemap::chattercallback, this);
31 
32  const bool load = true;
34  MyTh_opp_passiveJoint.robot_description_ = "robot_description"; // default
37 
38  //Get Server IP address
39  if (ros::param::has("~remap_MIA_th_opp"))
40  {
41  ros::param::get("~remap_MIA_th_opp", remap_th_opp);
42  }
43  else
44  {
45  remap_th_opp = false; // default value
46  ros::param::set("~remap_MIA_th_opp", remap_th_opp);
47 
48  }
49 
50 
51  }
52 
53  private:
54 
59 
63  ros::NodeHandle n;
64 
68  ros::Publisher pub;
69 
73  ros::Subscriber sub;
74 
78  std::string j_index_name ;
79 
83  std::string j_thumb_name ;
84 
88  std::string robot_description_ ;
89 
94 
95 
100  void chattercallback(const sensor_msgs::JointState::ConstPtr& msg)
101  {
102  int nJoints = msg->name.size();
103 
104  sensor_msgs::JointState sensor_msgs = *msg;
105 
106  int j_index = -1;
107  int j_th = -1;
108 
109  for(unsigned int j=0; j < nJoints; j++)
110  {
111  const std::string jname = sensor_msgs.name[j];
112  if (jname == j_index_name) {j_index = j;}
113  if (jname == j_thumb_name) {j_th = j;}
114  if(j_index > -1 && j_th > -1) {break;}
115  }
116 
117  // Remap
118 
119 
120  if (j_index > -1 && j_th >-1 && remap_th_opp == true)
121  {
122  double th_fle_pos = MyTh_opp_passiveJoint.GetThumbOppPosition(sensor_msgs.position[j_index]);
123  sensor_msgs.position[j_th] = th_fle_pos;
124  sensor_msgs.position[j_index] = std::abs(sensor_msgs.position[j_index]);
125 
126  }
127  else if (j_index > -1 && (j_th == -1 || remap_th_opp == false))
128  {
129  sensor_msgs.position[j_index] = std::abs(sensor_msgs.position[j_index]);
130  }
131 
132  pub.publish(sensor_msgs);
133 
134  }
135 
136 
137 };
138 
139 
140 
144 int main(int argc, char **argv)
145 {
146 
147  ros::init(argc, argv, "remap_mia_joint_states");
148 
149  myRemap myInstance;
150 
151 
152  ros::spin();
153 
154  return 0;
155 }
myRemap::pub
ros::Publisher pub
Publisher to publish remapped miajoint.
Definition: remap_mia_joint_states.cpp:68
myRemap::sub
ros::Subscriber sub
Subscriber to publish listen to raw mia joint states.
Definition: remap_mia_joint_states.cpp:73
mia_hand::thumb_opp_passive_joint::GetThumbOppPosition
double GetThumbOppPosition(double j_index_flex_pos)
Evaluate the position of the thumb opposition based on index flexion state.
Definition: mia_thumb_opp_passivejoints.cpp:38
myRemap::j_index_name
std::string j_index_name
Name of the index flex joint as declared in the URDF.
Definition: remap_mia_joint_states.cpp:78
myRemap::n
ros::NodeHandle n
Ros node varibale.
Definition: remap_mia_joint_states.cpp:63
main
int main(int argc, char **argv)
Main of the ROS node using the Remap class.
Definition: remap_mia_joint_states.cpp:144
myRemap::myRemap
myRemap()
Constructor.
Definition: remap_mia_joint_states.cpp:27
myRemap::chattercallback
void chattercallback(const sensor_msgs::JointState::ConstPtr &msg)
Callback of the subscriber.
Definition: remap_mia_joint_states.cpp:100
mia_hand::thumb_opp_passive_joint::j_thumb_name
std::string j_thumb_name
Name of the thumb_opp joint specified in the URDF.
Definition: mia_thumb_opp_passivejoints.h:92
mia_thumb_opp_passivejoints.h
mia_hand::thumb_opp_passive_joint
thumb_opp_passive_joints class.
Definition: mia_thumb_opp_passivejoints.h:36
mia_hand::thumb_opp_passive_joint::init
void init(const bool LoadURDFInfo_=false)
Initialize class.
Definition: mia_thumb_opp_passivejoints.cpp:19
myRemap::robot_description_
std::string robot_description_
Name of parameter link to the URDF.
Definition: remap_mia_joint_states.cpp:88
myRemap::j_thumb_name
std::string j_thumb_name
Name of the thumb opp joint as declared in the URDF.
Definition: remap_mia_joint_states.cpp:83
myRemap
Class to remap Mia JointState.
Definition: remap_mia_joint_states.cpp:19
myRemap::MyTh_opp_passiveJoint
mia_hand::thumb_opp_passive_joint MyTh_opp_passiveJoint
Class describing the thumb_opp and index flex passive joint.
Definition: remap_mia_joint_states.cpp:93
mia_hand::thumb_opp_passive_joint::robot_description_
std::string robot_description_
Name of parameter describing the robot description loaded in the ROS parameter server.
Definition: mia_thumb_opp_passivejoints.h:98
myRemap::remap_th_opp
bool remap_th_opp
Bool True to remap also the thumb opp position.
Definition: remap_mia_joint_states.cpp:58
mia_hand::thumb_opp_passive_joint::j_index_name
std::string j_index_name
Name of the index_fle joint specified in the URDF.
Definition: mia_thumb_opp_passivejoints.h:86