void update_mavlink(void){
mavlink_message_t msg;
mavlink_status_t status;
if(ser.available()){
uint16_t num_bytes = ser.available();
uint8_t buffer[num_bytes];
ser.read(buffer, num_bytes);
for(uint16_t i = 0; i < num_bytes; i++){
if (mavlink_parse_char(MAVLINK_COMM_0, buffer[i], &msg, &status)) {
handleMessage(&msg, MAVLINK_COMM_0);
}
}
}
}
void receive(){
if(!checkConnection()){
return;
}
// reads:
// 3 bytes:
// 1 byte to compare with START_BYTE
// 1 bytes into _msg->len
// 1 byte into _msg->type
// len+1 bytes:
// len bytes into _msg->data
// 1 byte to compare with _msg->checksum
size_t amount;
while((amount=port.available()) > 0){
if(_pos > 0){
amount = min((unsigned int)amount, _msg->length + 3 - _pos);
port.read(&(_msg->data[_pos]), amount);
_pos += amount;
if(_pos == _msg->length + 3 && port.available()){
uint8_t checksum;
port.read(&checksum, 1);
_msg->calcChecksum();
if(checksum == _msg->checksum){
parse(_msg);
}else{
ROS_WARN_THROTTLE(1, "Serial: Checksum mismatch: %u != %u", checksum, _msg->checksum);
}
resetMessage();
}
}else if(amount >= 3){
uint8_t b;
port.read(&b, 1);
if(b != START_BYTE){
ROS_WARN_THROTTLE(1, "Serial: Expected start byte, got this instead: %u", b);
return;
}
uint8_t d[2];
port.read(d, 2);
uint16_t len = d[0];
if(len > 0){
_msg = new Message(len - 1, d[1]); // TODO catch exception?
_pos = 3;
}
}
}
}
// Get all bytes available
bool getBytes(char * buffer, int& buf_len) {
bool res = false;
//buf_len = 0;
try {
while (true) {
int res = 0;
size_t available = m_serial.available();
if (!available) {
break;
} else {
res = m_serial.read((uint8_t*)buffer + buf_len, available);
}
if (res <= 0) {
break;
}
buf_len += res;
m_last_read_time = Datapoint::get_current_time();
}
} catch (...) {
if (debug) {
std::cout << "exception thrown while wattsup reading" << std::endl;
}
close();
}
if (buf_len > 0) {
res = true;
}
return res;
}
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;
}