35 #include <tf/transform_broadcaster.h>
37 #include <sys/types.h>
49 #include <haptic_force/Force.h>
51 #include <pressure_cells/Cop.h>
57 void COPCallBk (
const pressure_cells::Cop &COP );
63 int main (
int argc,
char **argv )
66 ros::init( argc, argv,
"cell_force" );
68 ros::NodeHandle n(
"~");
70 ros::Publisher pub_force = n.advertise< haptic_force::Force >(
"/force", 1000 );
72 ros::Subscriber sub_cop = n.subscribe (
"/cop_left_right", 1000,
COPCallBk );
74 ros::Rate loop_rate( 1.5*1300 );
78 int pid = getpid(), rpid;
80 boost::format fmt(
"sudo renice -10 %d");
83 rpid = std::system(fmt.str().c_str());
90 force.header.stamp = ros::Time::now ( );
91 pub_force.publish(
force );
123 force.force[0]=( (2.5 - (-2.5)) / (0.14 - 0.0) )*YY - 2.5;
124 force.force[2]=( (2.5 - (-2.5)) / (0.046 - (-0.046)) )*XX;
126 if (abs(
force.force[2]) < 0.75)
130 else if((
force.force[2]) <= - 0.75)
132 force.force[2]+= 0.75;
134 else if((
force.force[2]) >= 0.75)
136 force.force[2]+= - 0.75;
139 if (abs(
force.force[0]) < 0.75)
143 else if((
force.force[0]) <= - 0.75)
145 force.force[0]+= 0.75;
147 else if((
force.force[0]) >= 0.75)
149 force.force[0]+= - 0.75;
haptic_force::Force force
int main(int argc, char **argv)
void COPCallBk(const pressure_cells::Cop &COP)