54 int main(
int argc,
char**argv)
57 ros::init(argc, argv,
"sensor_sharp_transformation_node");
59 ros::Subscriber sensor_msg_subs= n.subscribe(
"/snr/opto/sensor",1,
SensorCallback);
66 float d_front_left_array[8];
67 float d_front_right_array[8];
68 float d_back_left_array[8];
69 float d_back_right_array[8];
93 float z_mean=(dist_front_left + dist_front_right + dist_back_left + dist_back_right)/4;
94 float roll_front=atan2((dist_front_left - dist_front_right),1.27);
95 float roll_back=atan2((dist_back_left - dist_back_right),0.67);
97 float pitch=atan2((dist_back_left + dist_back_right)*0.5 - (dist_front_left + dist_front_right)*0.5,1.39*2);
98 static tf::TransformBroadcaster br;
99 tf::Transform transform;
100 transform.setOrigin( tf::Vector3(0.0, 0.0, z_mean) );
101 transform.setRotation(tf::createQuaternionFromRPY((roll_front+roll_back)*0.5, pitch, 0) );
102 br.sendTransform(tf::StampedTransform(transform,
sharp_msg.header.stamp,
"/ground",
"/atc/vehicle/center_car_axis" ));
int main(int argc, char **argv)
main
vector< double > measures
optoelectric::sensor_sharp_msg sharp_msg
void SensorCallback(const optoelectric::sensor_sharp_msg &msg)
SensorCallback.