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(); } }