bool Move::handshake(){
int counter = 0;
while(ros::ok()){
syncboard.flush();
syncboard.write("v");
std::string result = syncboard.readline();
if (result.length() > 0){
ROS_INFO("Connected to syncboard.");
return true;
}
if(counter++ > 50){
ROS_WARN_ONCE("Connecting to syncboard is taking longer than expected.");
}
ros::Rate(10).sleep();
}
ROS_WARN("Syncboard handshake failed.");
return false;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "doa_rssi_server");
ros::NodeHandle n2;
tf::TransformListener listener;
ros::Rate r(10);
ros::ServiceServer service = n2.advertiseService("get_doa_rssi", add);
ros::Publisher marker_pub_path = n2.advertise<visualization_msgs::Marker>("beacon_point", 10);
tf::StampedTransform transformMtoR;
tf::StampedTransform transform;
string dataString;
float doarssi[3];
bool doarssiready = false;
//ros::spin();//a blocking call duhh!
ROS_INFO("ready to give DOA and RSSI!!");
visualization_msgs::Marker markerDOA,markerIntersections,markerBeacon1,markerBeacon2,markerPath;
markerDOA.header.frame_id = "/map";
markerDOA.header.stamp = ros::Time::now();
markerDOA.ns = "markers";
markerDOA.action = visualization_msgs::Marker::ADD;
markerDOA.pose.orientation.w = 1.0;
markerDOA.id = 0;
markerDOA.type = visualization_msgs::Marker::POINTS;
//########POINTS markers use x and y scale for width/height respectively
markerDOA.scale.x = 1.2;
markerDOA.scale.y = 1.2;
markerDOA.scale.z = 0.5;
markerDOA.color.r = 1.0f;
markerDOA.color.b = 1.0f;
markerDOA.color.a = 1.0;
geometry_msgs::Point marker_point;
marker_point.x = 19.0;
marker_point.y = 19.0;
marker_point.z = 0.0;
markerDOA.points.push_back(marker_point);
marker_pub_path.publish(markerDOA);
while(n2.ok())
{
#ifndef SDR
try{
listener.lookupTransform("base_link", "beacon",
ros::Time(0), transform);
}
catch (tf::TransformException &ex) {
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
continue;
}
phi = atan2f(transform.getOrigin().y(),transform.getOrigin().x());
rssi = 1/(transform.getOrigin().x()*transform.getOrigin().x()+transform.getOrigin().y()*transform.getOrigin().y());
#else
if(robotserial.available())
{
dataString = robotserial.readline(100,"\n");
cout << "odom received: "<< dataString << endl;
char * dataStringArray = new char [dataString.length()+1];
std::strcpy (dataStringArray, dataString.c_str());
//printf("%s\n",dataStringArray);
// dataStringArray now contains a c-string copy of str
char * token = std::strtok (dataStringArray,",");
char *unconverted;
int i = 0;
if(*token=='t')
{
while (token!=0)
{
//std::cout << token << "-" << atoi(token) <<'\n';
doarssi[i] = atof(token);
token = std::strtok(NULL,",");
i++;
}
doarssiready = true;
}
}
#endif
if(doarssiready==true)
{
phi = doarssi[1];
rssi = doarssi[2];
doarssiready = false;
}
ros::spinOnce();
marker_pub_path.publish(markerDOA);
}
return 0;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "IMU_Node2");
ros::NodeHandle n;
imu_pub = n.advertise<sensor_msgs::Imu>("/imu/data",100);
// Change the next line according to your port name and baud rate
try
{
if(device.isOpen())
{
ROS_INFO("Port is opened");
}
}
catch(serial::SerialException& e)
{
ROS_FATAL("Failed to open the serial port!!!");
ROS_BREAK();
}
ros::Rate r(50);
while(ros::ok())
{
// Get the reply, the last value is the timeout in ms
try{
std::string reply;
// ros::Duration(0.005).sleep();
device.readline(reply);
Roll = strtod(reply.c_str(),&pRoll)/2;
Pitch = strtod(pRoll,&pRoll)/2;
Yaw = strtod(pRoll,&pRoll)/2;
GyroX = strtod(pRoll,&pRoll);
GyroY = strtod(pRoll,&pRoll);
GyroZ = strtod(pRoll,&pRoll);
AccX = strtod(pRoll,&pRoll);
AccY = strtod(pRoll,&pRoll);
AccZ = strtod(pRoll,&pRoll);
Mx = strtod(pRoll,&pRoll);
My = strtod(pRoll,&pRoll);
Mz = strtod(pRoll,NULL);
ROS_INFO("Euler %.2f %.2f %.2f",Roll,Pitch,Yaw);
sensor_msgs::Imu imu;
geometry_msgs::Quaternion Q;
Q = tf::createQuaternionMsgFromRollPitchYaw(Roll*-1,Pitch*-1,Yaw*-1);
imu.orientation.x = 0;
imu.orientation.y = 0;
imu.orientation.z = 0;
imu.orientation.w = 1;
LinearX = (AccX/256)*9.806;
LinearY = (AccY/256)*9.806;
LinearZ = (AccZ/256)*9.806;
imu.angular_velocity.x = GyroX*-1*0.07*0.01745329252;;
imu.angular_velocity.y = GyroY*0.07*0.01745329252;;
imu.angular_velocity.z = GyroZ*-1*0.07*0.01745329252;;
imu.linear_acceleration.x = LinearX*-1;
imu.linear_acceleration.y = LinearY;
imu.linear_acceleration.z = LinearZ*-1;
imu.header.stamp = ros::Time::now();
imu.header.frame_id = "imu";
imu_pub.publish(imu);
}
catch(serial::SerialException& e)
{
ROS_INFO("Error %s",e.what());
}
r.sleep();
}
}
