45 #include <boost/asio.hpp>
46 #include <boost/array.hpp>
47 #include <boost/thread.hpp>
48 #include <boost/asio/ip/tcp.hpp>
49 #include <boost/thread/mutex.hpp>
50 #include <boost/thread/locks.hpp>
51 #include <boost/thread/thread.hpp>
55 #include <visualization_msgs/Marker.h>
56 #include <visualization_msgs/MarkerArray.h>
58 #include <Eigen/Dense>
59 #include <Eigen/Geometry>
61 #include <serialcom/SerialCom.h>
63 #include <opencv2/core/core.hpp>
64 #include <opencv2/highgui/highgui.hpp>
66 #include <sensor_msgs/Range.h>
67 #include <ros/package.h>
68 #include <geometry_msgs/Vector3.h>
71 using namespace boost;
117 cv::FileStorage parameters(parameters_file.c_str(),cv::FileStorage::READ);
119 number_sensors = (int)parameters[
"number_sensors"];
123 for(uint s = 0;s<number_sensors;s++)
127 pos(0) = parameters[
"x"+to_string(s)];
128 pos(1) = parameters[
"y"+to_string(s)];
130 sensors_positions.push_back(pos);
132 Eigen::VectorXd curve(4);
134 curve(0) = parameters[
"p1_"+to_string(s)];
135 curve(1) = parameters[
"p2_"+to_string(s)];
136 curve(2) = parameters[
"p3_"+to_string(s)];
137 curve(3) = parameters[
"p4_"+to_string(s)];
139 sensors_curve_parameters.push_back(curve);
142 pitch_bias = parameters[
"pitch_bias"];
143 roll_bias = parameters[
"roll_bias"];
159 Eigen::VectorXd g(number_sensors);
160 g = Eigen::VectorXd::Ones(number_sensors);
164 Eigen::MatrixXd F(number_sensors,3);
166 for(uint i=0;i<number_sensors;i++)
168 F(i,0) = sensors_positions[i](0);
169 F(i,1) = sensors_positions[i](1);
173 Eigen::VectorXd r(3);
175 r = ((F.transpose()*F).inverse() * F.transpose())*g;
177 double zp = ((-r(0)/r(2))*sensors_positions[0](0)) - ((r(1)/r(2))*sensors_positions[0](1));
179 double d = values[0] - zp;
209 return ((180./M_PI)*(atan2(d,(d*c/a)))) - pitch_bias;
224 return ((180./M_PI)*(atan2(d,(d*c/b)))) - roll_bias;
231 vector<Eigen::Vector2d> sensors_positions;
232 vector<Eigen::VectorXd> sensors_curve_parameters;
257 string parameters_url;
259 nh_.param(
"parameters",parameters_url,std::string(
"no parameters file"));
261 cout <<
"url: " << parameters_url << endl;
263 s.readParameters(parameters_url);
275 new_data.resize(8,
false);
277 for(uint i=0;i<8;i++)
281 sharp_subscriber.push_back(sub);
287 pose_publisher = nh_.advertise<geometry_msgs::Vector3>(
"pose_orientation", 1000);
288 car_mark_pub = nh_.advertise<visualization_msgs::Marker>(
"atlascar_ori",1000);
301 sharp_values[i]=msg->range;
312 pose.x=pose.x*3.14/180;
313 pose.y=pose.y*3.14/180;
314 pose.z=pose.z*3.14/180;
316 car_marker.header.frame_id =
"base_link";
318 car_marker.id = 8484;
319 car_marker.ns =
"atlas";
321 car_marker.type = visualization_msgs::Marker::MESH_RESOURCE;
322 car_marker.action = visualization_msgs::Marker::ADD;
325 car_marker.pose.position.x = 0;
326 car_marker.pose.position.y = 0;
327 car_marker.pose.position.z = 0.4;
329 car_marker.scale.x = 1;
330 car_marker.scale.y = 1;
331 car_marker.scale.z = 1;
333 car_marker.pose.orientation.x = sin(pose.x/2)*cos(pose.y/2)*cos(pose.z/2) - cos(pose.x/2)*sin(pose.y/2)*sin(pose.z/2);
334 car_marker.pose.orientation.y = cos(pose.x/2)*sin(pose.y/2)*cos(pose.z/2) + sin(pose.x/2)*cos(pose.y/2)*sin(pose.z/2);
335 car_marker.pose.orientation.z = cos(pose.x/2)*cos(pose.y/2)*sin(pose.z/2) - sin(pose.x/2)*sin(pose.y/2)*cos(pose.z/2);
336 car_marker.pose.orientation.w = cos(pose.x/2)*cos(pose.y/2)*cos(pose.z/2) + sin(pose.x/2)*sin(pose.y/2)*sin(pose.z/2);
340 car_marker.color.r = 0.0f;
341 car_marker.color.g = 0.0f;
342 car_marker.color.b = 0.0f;
343 car_marker.color.a = 0.0f;
345 car_marker.mesh_use_embedded_materials=1;
347 car_marker.mesh_resource =
"package://optoelectric/ship/FordEscort_ori.DAE";
349 car_marker.lifetime = ros::Duration();
367 for(
bool v: new_data)
379 vector<double> values;
380 for(uint i=0;i<8;i++)
382 double val = sharp_values[i];
383 values.push_back(val);
387 Plane plane = s.getPlane(values);
389 pitch = s.getPitch(plane);
390 roll = s.getRoll(plane);
401 cout <<
"pitch: " << pitch <<
" roll: " << roll << endl;
403 geometry_msgs::Vector3 posee;
405 if(pitch < 45 && roll < 45)
413 pose_publisher.publish(posee);
416 car_marker.header.stamp = ros::Time::now();
417 car_mark_pub.publish(car_marker);
422 new_data.resize(8,
false);
441 vector<ros::Subscriber> sharp_subscriber;
444 vector<bool> new_data;
452 int main(
int argc,
char* argv[])
455 ros::init(argc, argv,
"preception_orientation_base");
457 ros::NodeHandle nh(
"~");
SharpMath()
Class constructor.
int main(int argc, char *argv[])
main Init ros and stays in loop forever.
ros::Publisher pose_publisher
OptoelectricVehiclePose(const ros::NodeHandle &nh)
Class constructor Init the serial port, open the txt file and the yaml file.
void carMarker(geometry_msgs::Vector3 pose)
carMarker Function responsible publish the car marker.
void loop()
loop Function responsible to put the program running forever.
Plane()
Class constructor.
void sharpCallback(const sensor_msgs::Range::ConstPtr msg, int i)
sharpCallback Function responsible evaluate is the message is valid.
double getPitch(Plane p)
getPitch Function responsible for calculate the plane pitch.
ros::Publisher car_mark_pub
Plane getPlane(vector< double > values)
getPlane Function responsible for calculate the plane acording to the given points.
visualization_msgs::Marker car_marker
double getRoll(Plane p)
getRoll Function responsible for calculate the plane roll.
void readParameters(string parameters_file)
readParameters Function responsible for read the parameters from yaml file.
void setupMessaging()
setupMessaging Function responsible for creat the publishers.