mia_hand_ros_control  rel 1.0.0
test_node.cpp
Go to the documentation of this file.
1 #include "ros/ros.h"
2 #include "std_msgs/String.h"
3 #include "std_msgs/Float64.h"
4 
5 #include <vector>
6 #include <sstream>
7 
8 #include <transmission_interface/transmission_interface.h>
9 #include <transmission_interface/simple_transmission.h>
14 
15 
16 #include <iostream> // to be deleted
17 
18 int main(int argc, char **argv)
19 {
20 
21 
22  ros::init(argc, argv, "test_node");
23 
24 
25  ros::NodeHandle n;
26 
27 
28  ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
29 
30  ros::Rate loop_rate(10);
31 
32 
33  int count = 0;
34 
35 
36  // Transmission MRL
37  // Raw data
38  double a_mrl_pos;
39  double j_mrl_pos;
40 
41  double a_mrl_spe;
42  double j_mrl_spe;
43 
44 
45  // Transmission MRL
47  std::vector<transmission_interface::Transmission*> mio;
48  mio.resize(3);
49  mio[0] = & ThfleTrans;
50 
52 
53 
54 
55  // Transmission
56 
57 
58  // Wrap raw data MRL
59  transmission_interface::ActuatorData a_mrl_data;
60  a_mrl_data.position.push_back(&a_mrl_pos);
61  a_mrl_data.velocity.push_back(&a_mrl_spe);
62 
63  transmission_interface::JointData j_mrl_data;
64  j_mrl_data.position.push_back(&j_mrl_pos);
65  j_mrl_data.velocity.push_back(&j_mrl_spe);
66 
67 
68 
69  // Transmission interface
70  transmission_interface::ActuatorToJointPositionInterface act_to_jnt_mrl_pos;
71  act_to_jnt_mrl_pos.registerHandle(transmission_interface::ActuatorToJointPositionHandle("MrlTrans", &MrlTrans, a_mrl_data, j_mrl_data));
72 
73  transmission_interface::JointToActuatorPositionInterface jnt_to_act_mrl_pos;
74  jnt_to_act_mrl_pos.registerHandle(transmission_interface::JointToActuatorPositionHandle("MrlTrans", &MrlTrans, a_mrl_data, j_mrl_data));
75 
76  transmission_interface::ActuatorToJointVelocityInterface act_to_jnt_mrl_spe;
77  act_to_jnt_mrl_spe.registerHandle(transmission_interface::ActuatorToJointVelocityHandle("MrlTrans", &MrlTrans, a_mrl_data, j_mrl_data));
78 
79  transmission_interface::JointToActuatorVelocityInterface jnt_to_act_mrl_spe;
80  jnt_to_act_mrl_spe.registerHandle(transmission_interface::JointToActuatorVelocityHandle("MrlTrans", &MrlTrans, a_mrl_data, j_mrl_data));
81 
82 
83 
84  // Transmission INDEX
85  // Raw data
86  double a_index_pos; // state
87  double j_index_pos;
88 
89  double a_index_spe; // state
90  double j_index_spe;
91 
92  double a_index_pos_cmd; // cmd
93  double a_index_spe_cmd; // cmd
94 
95 
96  transmission_interface::MiaIndexTransmission IndexTrans; // index trasmission
97 
98 
99  // Wrap raw data MRL
100  transmission_interface::ActuatorData a_index_data;
101  a_index_data.position.push_back(&a_index_pos); // state
102  a_index_data.velocity.push_back(&a_index_spe);
103 
104  transmission_interface::ActuatorData a_index_cmd;
105  a_index_cmd.position.push_back(&a_index_pos_cmd); // cmd
106  a_index_cmd.velocity.push_back(&a_index_spe_cmd);
107 
108  transmission_interface::JointData j_index_data;
109  j_index_data.position.push_back(&j_index_pos);
110  j_index_data.velocity.push_back(&j_index_spe);
111 
112 
113 
114  // Transmission interface
116  act_to_jnt_index_pos.registerHandle(transmission_interface::MiaActuatorToJointPositionHandle("IndexTrans", &IndexTrans, a_index_data, j_index_data));
117 
119  jnt_to_act_index_pos.registerHandle(transmission_interface::MiaJointToActuatorPositionHandle("IndexTrans", &IndexTrans, a_index_cmd, j_index_data,a_index_data));
120 
122  act_to_jnt_index_spe.registerHandle(transmission_interface::MiaActuatorToJointVelocityHandle("IndexTrans", &IndexTrans, a_index_data, j_index_data));
123 
125  jnt_to_act_index_spe.registerHandle(transmission_interface::MiaJointToActuatorVelocityHandle("IndexTrans", &IndexTrans, a_index_cmd, j_index_data ,a_index_data));
126 
127 
128  while (ros::ok())
129  {
130 
131  //******* MRL ****** //
132 
133  std::cout<< "Give me MRL actuator pos data:\n";
134  std::cin>> a_mrl_pos;
135 
136  // Propagate actuator position to joint space
137  act_to_jnt_mrl_pos.propagate();
138  std::cout<< "j_mrl_angle: " << j_mrl_pos;
139  std::cout<< std::endl;
140 
141 
142  std::cout<< "Give me MRL joint positin data:\n";
143  std::cin>> j_mrl_pos;
144  std::cout<< std::endl;
145 
146  // Propagate actuator position to joint space
147  jnt_to_act_mrl_pos.propagate();
148  std::cout<< "j_mrl_pos counts: " << a_mrl_pos ;
149  std::cout<< std::endl;
150 
151  // Velocity
152  std::cout<< "Give me MRL actuator speed data:\n";
153  std::cin>> a_mrl_spe;
154  std::cout<< std::endl;
155 
156  // Propagate actuator position to joint space
157  act_to_jnt_mrl_spe.propagate();
158  std::cout<< "j_mrl_angular_speed: " << j_mrl_spe ;
159  std::cout<< std::endl;
160 
161  std::cout<< "Give me MRL joint angular velocity:\n";
162  std::cin>> j_mrl_spe ;
163  std::cout<< std::endl;
164 
165  // Propagate actuator position to joint space
166  jnt_to_act_mrl_spe.propagate();
167  std::cout<< "j_mrl_spe counts: " << a_mrl_spe << std::endl;
168 
169 
170  //******* Index ****** //
171  /*
172  std::cout<< "Give me Index actuator pos data:\n";
173  std::cin>> a_index_pos;
174 
175  // Propagate actuator position to joint space
176  act_to_jnt_index_pos.propagate();
177  std::cout<< "j_index_angle: " << j_index_pos;
178  std::cout<< std::endl<< std::endl;
179 
180 
181  std::cout<< "Give me index joint positin data:\n";
182  std::cin>> j_index_pos;
183 
184  // Propagate actuator position to joint space
185  jnt_to_act_index_pos.propagate();
186  std::cout<< "j_index_pos counts: " << a_index_pos_cmd ;
187  std::cout<< std::endl<< std::endl;
188 
189  // Velocity
190  std::cout<< "Give me index actuator speed data:\n";
191  std::cin>> a_index_spe;
192 
193 
194  // Propagate actuator position to joint space
195  act_to_jnt_index_spe.propagate();
196  std::cout<< "j_index_angular_speed: " << j_index_spe <<" at pos: " << a_index_pos ;
197  std::cout<< std::endl<< std::endl;
198 
199  std::cout<< "Give me index joint angular velocity:\n";
200  std::cin>> j_index_spe ;
201 
202 
203  // Propagate actuator position to joint space
204  jnt_to_act_index_spe.propagate();
205  std::cout<< "j_index_spe counts: " << a_index_spe_cmd <<" at pos: " << a_index_pos << std::endl<< std::endl;
206 
207  */
208 
209 
210 
211  std_msgs::String msg;
212 
213  std::stringstream ss;
214  ss << "\n hello world " << count;
215  msg.data = ss.str();
216 
217 
218 
219  chatter_pub.publish(msg);
220 
221  ros::spinOnce();
222 
223  loop_rate.sleep();
224  ++count;
225  }
226 
227 
228  return 0;
229 }
transmission_interface::MiaActuatorToJointPositionHandle
Class handling for propagating actuator positions to joint positions for a given MiaIndexTransmission...
Definition: mia_transmission_interface.h:243
mia_transmission_interface.h
transmission_interface::MiaActuatorToJointVelocityHandle
Class handling for propagating actuator velocity to joint velocity for a given MiaIndexTransmission.
Definition: mia_transmission_interface.h:271
transmission_interface::MiaMrlTransmission
Implementation of the linear Mia mrl Transmission mapping the positions and velocity from the joint s...
Definition: mia_mrl_transmission.h:60
mia_thfle_transmission.h
mia_mrl_transmission.h
transmission_interface::MiaActuatorToJointPositionInterface
Definition: mia_transmission_interface.h:422
transmission_interface::MiaIndexTransmission
Implementation of the no-linear Mia Index Transmission.
Definition: mia_index_transmission.h:34
main
int main(int argc, char **argv)
Definition: test_node.cpp:18
transmission_interface::MiaJointToActuatorPositionHandle
Class handling for propagating joint positions to actuator positions for a given MiaIndexTransmission...
Definition: mia_transmission_interface.h:334
transmission_interface::MiaJointToActuatorVelocityHandle
Class handling for propagating joint velocities to actuator velocities for a given MiaIndexTransmissi...
Definition: mia_transmission_interface.h:365
mia_index_transmission.h
transmission_interface::MiaJointToActuatorVelocityInterface
Definition: mia_transmission_interface.h:431
transmission_interface::MiaTransmissionInterface::propagate
void propagate()
Definition: mia_transmission_interface.h:407
transmission_interface::MiaJointToActuatorPositionInterface
Definition: mia_transmission_interface.h:429
transmission_interface::MiaThfleTransmission
Implementation of the linear Mia thumb flexion transmission mapping the positions and velocity from t...
Definition: mia_thfle_transmission.h:60
transmission_interface::MiaActuatorToJointVelocityInterface
Definition: mia_transmission_interface.h:424