38 #include <tf/transform_listener.h>
40 #include <visualization_msgs/Marker.h>
41 #include <visualization_msgs/MarkerArray.h>
43 #include <haptic_force/Force.h>
44 #include <pressure_cells/Cop.h>
45 #include <phantom_control/State.h>
46 #include <humanoid_control/Humanoid.h>
47 #include <pressure_cells/SenVal.h>
59 arduino_1_values_report_name_ = ros::names::remap(
"arduino_1_values_report");
60 arduino_2_values_report_name_ = ros::names::remap(
"arduino_2_values_report");
62 cop_left_right_report_name_ = ros::names::remap(
"cop_left_right_report");
63 cop_left_report_name_ = ros::names::remap(
"cop_left_report");
64 cop_right_report_name_ = ros::names::remap(
"cop_right_report");
66 force_report_name_ = ros::names::remap(
"force_report_name_");
67 humanoid_state_report_name_ = ros::names::remap(
"humanoid_state_report");
68 phantom_state_report_name_ = ros::names::remap(
"phantom_state_report");
70 remove(arduino_1_values_report_name_.c_str());
71 remove(arduino_2_values_report_name_.c_str());
73 remove(cop_left_right_report_name_.c_str());
74 remove(cop_left_report_name_.c_str());
75 remove(cop_right_report_name_.c_str());
77 remove(force_report_name_.c_str());
78 remove(humanoid_state_report_name_.c_str());
79 remove(phantom_state_report_name_.c_str());
81 arduino_1_handler_ = n_.subscribe<pressure_cells::SenVal> (
"/arduino_1_values",1000,boost::bind(&
Conversion::ArduinoHandler,
this,_1,arduino_1_values_report_name_));
82 arduino_2_handler_ = n_.subscribe<pressure_cells::SenVal> (
"/arduino_2_values",1000,boost::bind(&
Conversion::ArduinoHandler,
this,_1,arduino_2_values_report_name_));
84 cop_left_right_handler_ = n_.subscribe<pressure_cells::Cop> (
"/cop_left_right",1000,boost::bind(&
Conversion::COPHandler,
this,_1,cop_left_right_report_name_));
85 cop_left_handler_ = n_.subscribe<pressure_cells::Cop> (
"/cop_left",1000,boost::bind(&
Conversion::COPHandler,
this,_1,cop_left_report_name_));
86 cop_right_handler_ = n_.subscribe<pressure_cells::Cop> (
"/cop_right",1000,boost::bind(&
Conversion::COPHandler,
this,_1,cop_right_report_name_));
100 void ArduinoHandler(
const pressure_cells::SenVal::ConstPtr& ard_values,
string report_name)
103 report_ard.open(report_name.c_str(),ios::app);
105 boost::format fm(
"%.6f");
106 fm % ard_values->header.stamp.toSec();
108 report_ard<<ard_values->header.seq<<
" "<<fm.str()<<
" "<< ard_values->sen1<<
" "<< ard_values->sen2 <<
" "<< ard_values->sen3 <<
" "<<ard_values->sen4<<endl;
113 void COPHandler(
const pressure_cells::Cop::ConstPtr& cop_values,
string report_name)
116 report_cop.open(report_name.c_str(),ios::app);
118 boost::format fm(
"%.6f");
119 fm % cop_values->header.stamp.toSec() ;
121 report_cop<<cop_values->header.seq<<
" "<<fm.str()<<
" "<< cop_values->copx <<
" "<<cop_values->copy<<endl;
128 ofstream report_force;
129 report_force.open(force_report_name_.c_str(),ios::app);
131 boost::format fm(
"%.6f");
132 fm % force_values->header.stamp.toSec();
134 report_force<<force_values->header.seq<<
" "<<fm.str()<<
" "<< force_values->force[0]<<
" "<< force_values->force[1]<<
" "<< force_values->force[2]<<endl;
136 report_force.close();
142 ofstream report_humanoid;
143 report_humanoid.open(humanoid_state_report_name_.c_str(),ios::app);
145 boost::format fm(
"%.6f");
146 fm % humanoid_values->header.stamp.toSec();
148 report_humanoid<<humanoid_values->header.seq<<
" "<<fm.str();
150 for (
int i = 0; i < 12; i++)
152 report_humanoid<<
" "<<humanoid_values->speed_wanted[i];
154 for (
int i = 0; i < 12; i++)
156 report_humanoid<<
" "<<humanoid_values->joint_now[i];
158 for (
int i = 0; i < 12; i++)
160 report_humanoid<<
" "<<humanoid_values->joint_wanted[i];
163 report_humanoid<<endl;
165 report_humanoid.close();
171 ofstream report_phantom;
172 report_phantom.open(phantom_state_report_name_.c_str(),ios::app);
174 boost::format fm(
"%.6f");
175 fm % phantom_values->header.stamp.toSec();
177 report_phantom<<phantom_values->header.seq<<
" "<<fm.str();
179 for (
int i = 0; i < 3; i++)
181 report_phantom<<
" "<<phantom_values->position[i];
183 for (
int i = 0; i < 3; i++)
185 report_phantom<<
" "<<phantom_values->rot[i];
187 for (
int i = 0; i < 3; i++)
189 report_phantom<<
" "<<phantom_values->joints[i];
191 for (
int i = 0; i < 3; i++)
193 report_phantom<<
" "<<phantom_values->force[i];
195 for (
int i = 0; i < 2; i++)
197 report_phantom<<
" "<<phantom_values->buttons[i];
200 report_phantom<<
" "<<phantom_values->home;
202 for (
int i = 0; i < 3; i++)
204 report_phantom<<
" "<<phantom_values->home_pos[i];
207 report_phantom<<endl;
209 report_phantom.close();
240 ros::init(argc, argv,
"bag2txt");
244 cout<<
"spining ..."<<endl;
string cop_left_right_report_name_
ros::Subscriber cop_right_handler_
string force_report_name_
string phantom_state_report_name_
void HumanoidHandler(const humanoid_control::HumanoidPtr &humanoid_values)
ros::Subscriber phantom_state_handler_
void ForceHandler(const haptic_force::ForcePtr &force_values)
string humanoid_state_report_name_
string cop_right_report_name_
ros::Subscriber force_report_handler_
ros::Subscriber arduino_1_handler_
void ArduinoHandler(const pressure_cells::SenVal::ConstPtr &ard_values, string report_name)
ros::Subscriber humanoid_state_handler_
void COPHandler(const pressure_cells::Cop::ConstPtr &cop_values, string report_name)
string arduino_2_values_report_name_
void PhantomHandler(const phantom_control::StatePtr &phantom_values)
ros::Subscriber arduino_2_handler_
ros::Subscriber cop_left_handler_
string arduino_1_values_report_name_
int main(int argc, char **argv)
string cop_left_report_name_
ros::Subscriber cop_left_right_handler_