2 #include "std_msgs/String.h"
3 #include "std_msgs/Float64.h"
8 #include <transmission_interface/transmission_interface.h>
9 #include <transmission_interface/simple_transmission.h>
18 int main(
int argc,
char **argv)
22 ros::init(argc, argv,
"test_node");
28 ros::Publisher chatter_pub = n.advertise<std_msgs::String>(
"chatter", 1000);
30 ros::Rate loop_rate(10);
47 std::vector<transmission_interface::Transmission*> mio;
49 mio[0] = & ThfleTrans;
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);
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);
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));
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));
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));
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));
92 double a_index_pos_cmd;
93 double a_index_spe_cmd;
100 transmission_interface::ActuatorData a_index_data;
101 a_index_data.position.push_back(&a_index_pos);
102 a_index_data.velocity.push_back(&a_index_spe);
104 transmission_interface::ActuatorData a_index_cmd;
105 a_index_cmd.position.push_back(&a_index_pos_cmd);
106 a_index_cmd.velocity.push_back(&a_index_spe_cmd);
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);
133 std::cout<<
"Give me MRL actuator pos data:\n";
134 std::cin>> a_mrl_pos;
138 std::cout<<
"j_mrl_angle: " << j_mrl_pos;
139 std::cout<< std::endl;
142 std::cout<<
"Give me MRL joint positin data:\n";
143 std::cin>> j_mrl_pos;
144 std::cout<< std::endl;
147 jnt_to_act_mrl_pos.propagate();
148 std::cout<<
"j_mrl_pos counts: " << a_mrl_pos ;
149 std::cout<< std::endl;
152 std::cout<<
"Give me MRL actuator speed data:\n";
153 std::cin>> a_mrl_spe;
154 std::cout<< std::endl;
157 act_to_jnt_mrl_spe.propagate();
158 std::cout<<
"j_mrl_angular_speed: " << j_mrl_spe ;
159 std::cout<< std::endl;
161 std::cout<<
"Give me MRL joint angular velocity:\n";
162 std::cin>> j_mrl_spe ;
163 std::cout<< std::endl;
166 jnt_to_act_mrl_spe.propagate();
167 std::cout<<
"j_mrl_spe counts: " << a_mrl_spe << std::endl;
211 std_msgs::String msg;
213 std::stringstream ss;
214 ss <<
"\n hello world " << count;
219 chatter_pub.publish(msg);