|
@@ -13,7 +13,8 @@
|
|
#include <cmath>
|
|
#include <cmath>
|
|
#include <arpa/inet.h>
|
|
#include <arpa/inet.h>
|
|
#include <sys/socket.h>
|
|
#include <sys/socket.h>
|
|
-
|
|
|
|
|
|
+#include <fstream>
|
|
|
|
+using namespace std;
|
|
|
|
|
|
namespace robot{
|
|
namespace robot{
|
|
namespace localizationspace {
|
|
namespace localizationspace {
|
|
@@ -76,6 +77,9 @@ int Localization::Start() {
|
|
LOG(INFO)<<Name()<<" start start...";
|
|
LOG(INFO)<<Name()<<" start start...";
|
|
geometry_pub = node_handle_->advertise<localization::localMsg>(robot::common::Geometry, 1);
|
|
geometry_pub = node_handle_->advertise<localization::localMsg>(robot::common::Geometry, 1);
|
|
//TODO zhushidiao
|
|
//TODO zhushidiao
|
|
|
|
+// string str = "/home/madesheng/taskuidata.txt";
|
|
|
|
+// read_txt(str);
|
|
|
|
+// speed_count = 0;
|
|
// rack_test_timer_ = CreateTimer(ros::Duration(duration_rack), &Localization::RackTestOnTimer, this,false,false);
|
|
// rack_test_timer_ = CreateTimer(ros::Duration(duration_rack), &Localization::RackTestOnTimer, this,false,false);
|
|
// rack_test_timer_.start();
|
|
// rack_test_timer_.start();
|
|
//TODO dakai
|
|
//TODO dakai
|
|
@@ -181,11 +185,43 @@ void Localization::RackTestOnTimer(const ros::TimerEvent &event)
|
|
localization.Latitude = 0;
|
|
localization.Latitude = 0;
|
|
localization.Height =0;
|
|
localization.Height =0;
|
|
localization.AngleDeviated = 0;
|
|
localization.AngleDeviated = 0;
|
|
- localization.Speed = 0;
|
|
|
|
|
|
+ if(speed_count <= speed_number -1)
|
|
|
|
+ {
|
|
|
|
+ localization.Speed = speeds[speed_count];
|
|
|
|
+ }
|
|
|
|
+ else
|
|
|
|
+ {
|
|
|
|
+ localization.Speed = 0;
|
|
|
|
+ }
|
|
geometry_pub.publish(localization);
|
|
geometry_pub.publish(localization);
|
|
|
|
+ speed_count ++;
|
|
|
|
|
|
}
|
|
}
|
|
-
|
|
|
|
|
|
+int Localization::read_txt(string &path)
|
|
|
|
+{
|
|
|
|
+ //ifstream myfile("/home/madesheng/data.txt");
|
|
|
|
+ ifstream myfile(path);
|
|
|
|
+ if (!myfile.is_open())
|
|
|
|
+ {
|
|
|
|
+ LOG(ERROR)<<Name()<< "open src File Error opening file" << endl;
|
|
|
|
+ return ROBOT_FAILTURE;
|
|
|
|
+ }
|
|
|
|
+ for (int i = 0; i < 18000; i++)
|
|
|
|
+ {
|
|
|
|
+ if(myfile.eof())
|
|
|
|
+ {
|
|
|
|
+ speed_number = i;
|
|
|
|
+ break;
|
|
|
|
+ }
|
|
|
|
+ myfile >> speeds[i];
|
|
|
|
+ //myfile >> temp;
|
|
|
|
+ //cout <<"char:"<< temp<< endl;
|
|
|
|
+ //cout <<"angle:"<< angles[i] <<endl;
|
|
|
|
+ }
|
|
|
|
+ cout << "speed NUMBER:" <<speed_number<<endl;
|
|
|
|
+ myfile.close();
|
|
|
|
+ return ROBOT_SUCCESS;
|
|
|
|
+}
|
|
bool Localization::IsOpen()
|
|
bool Localization::IsOpen()
|
|
{
|
|
{
|
|
//检测串口是否已经打开,并给出提示信息
|
|
//检测串口是否已经打开,并给出提示信息
|