Spiri
/
ROSBuzz_MISTLab
12
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

changes to pos tolarence value in navigation

This commit is contained in:
Gwaihir 2018-07-30 03:10:54 +00:00
parent 5665ac6ef2
commit 5d06389f5e
2 changed files with 6 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
buzz_scripts/include/act/states.bzz
View File

@ -10,10 +10,10 @@ include "utils/conversions.bzz"
TARGET_ALTITUDE = 15.0 # m. TARGET_ALTITUDE = 15.0 # m.
BVMSTATE = "TURNEDOFF" BVMSTATE = "TURNEDOFF"
PICTURE_WAIT = 20 # steps PICTURE_WAIT = 20 # steps
GOTO_MAXVEL = 2.5 # m/steps GOTO_MAXVEL = 0.5 # m/steps
GOTO_MAXDIST = 150 # m. GOTO_MAXDIST = 150 # m.
GOTODIST_TOL = 0.5 # m. GOTODIST_TOL = 0.8 # m.
GOTOANG_TOL = 0.1 # rad. GOTOANG_TOL = 0.2 # rad.
path_it = 0 path_it = 0
graphid = 0 graphid = 0
pic_time = 0 pic_time = 0
@ -96,7 +96,8 @@ function goto_gps(transf) {
else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination
transf() transf()
else { else {
LimitSpeed(m_navigation, 1.0) #LimitSpeed(m_navigation, 1.0)
log("NAVIGATION !!!! distance to target: ", math.vec2.length(m_navigation)," angle", math.vec2.angle(m_navigation) )
goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0) goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0)
} }
} }

7
src/roscontroller.cpp
View File

@ -614,7 +614,6 @@ with size......... | /
tmphead[1] = (uint16_t)message_number; tmphead[1] = (uint16_t)message_number;
get_logical_time(); get_logical_time();
uint32_t stime = (uint32_t)(logical_clock.toSec() * 100000); uint32_t stime = (uint32_t)(logical_clock.toSec() * 100000);
ROS_INFO("Sent stime %u for %u",stime,message_number);
memcpy((void*)(tmphead+2),(void*)&stime,sizeof(uint32_t)); memcpy((void*)(tmphead+2),(void*)&stime,sizeof(uint32_t));
uint64_t rheader[1]; uint64_t rheader[1];
payload_out.sysid = (uint8_t)robot_id; payload_out.sysid = (uint8_t)robot_id;
@ -623,7 +622,6 @@ with size......... | /
if(buzz_utility::get_timesync_state()){ if(buzz_utility::get_timesync_state()){
// prepare rosbuzz msg header // prepare rosbuzz msg header
tmphead[0] = (uint8_t)BUZZ_MESSAGE_TIME; tmphead[0] = (uint8_t)BUZZ_MESSAGE_TIME;
ROS_INFO("sent header : type %u mid %u time %u",tmphead[0],tmphead[1],tmphead[2]);
memcpy(rheader, tmphead, sizeof(uint64_t)); memcpy(rheader, tmphead, sizeof(uint64_t));
// push header into the buffer // push header into the buffer
payload_out.payload64.push_back(rheader[0]); payload_out.payload64.push_back(rheader[0]);
@ -641,7 +639,6 @@ with size......... | /
else{ else{
// prepare rosbuzz msg header // prepare rosbuzz msg header
tmphead[0] = (uint8_t)BUZZ_MESSAGE_WTO_TIME; tmphead[0] = (uint8_t)BUZZ_MESSAGE_WTO_TIME;
ROS_INFO("sent header : type %u mid %u time %u",tmphead[0],tmphead[1],tmphead[2]);
memcpy(rheader, tmphead, sizeof(uint64_t)); memcpy(rheader, tmphead, sizeof(uint64_t));
// push header into the buffer // push header into the buffer
payload_out.payload64.push_back(rheader[0]); payload_out.payload64.push_back(rheader[0]);
@ -1122,8 +1119,7 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg)
uint32_t temptime=0; uint32_t temptime=0;
memcpy(&temptime, r16head+2, sizeof(uint32_t)); memcpy(&temptime, r16head+2, sizeof(uint32_t));
float stime = (float)temptime/(float)100000; float stime = (float)temptime/(float)100000;
ROS_INFO("received stime %f for %u",stime, mid); if(debug) ROS_INFO("Received Msg: sent time %f for id %u",stime, mid);
ROS_INFO("Received header : type %u mid %u time %u",r16head[0],r16head[1],r16head[2]);
// Check for Updater message, if updater message push it into updater FIFO // Check for Updater message, if updater message push it into updater FIFO
if ((uint64_t)msg->payload64[0] == (uint64_t)UPDATER_MESSAGE) if ((uint64_t)msg->payload64[0] == (uint64_t)UPDATER_MESSAGE)
{ {
@ -1522,7 +1518,6 @@ void roscontroller::push_timesync_nei_msg(int nid, uint64_t nh, uint64_t nl, dou
buzz_utility::neighbor_time nt(nh, nl, ros::Time::now().toNSec(), buzz_utility::neighbor_time nt(nh, nl, ros::Time::now().toNSec(),
logical_clock.toNSec(), nr, relativeRate); logical_clock.toNSec(), nr, relativeRate);
neighbours_time_map.insert(make_pair(nid, nt)); neighbours_time_map.insert(make_pair(nid, nt));
ROS_INFO("SYNC MSG RECEIVED deltaLocal from %i",nid);
} }
uint64_t roscontroller::get_logical_time(){ uint64_t roscontroller::get_logical_time(){