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

open loop

This commit is contained in:
isvogor 2017-05-09 19:48:38 -04:00
parent 013bac1c5b
commit cbdc5a6db2
8 changed files with 945 additions and 641 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
include/roscontroller.h
View File

@ -70,6 +70,7 @@ private:
double DEFAULT_REFERENCE_LONGITUDE = -73.636075; double DEFAULT_REFERENCE_LONGITUDE = -73.636075;
double cur_pos[3]; double cur_pos[3];
double target[3];
double home[3]; double home[3];
double cur_rel_altitude; double cur_rel_altitude;
uint64_t payload; uint64_t payload;

2
launch/rosbuzz-testing-sitl.launch
View File

@ -27,7 +27,7 @@
<!-- run rosbuzz --> <!-- run rosbuzz -->
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" > <node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
<rosparam file="/home/ivan/catkin_ws/src/rosbuzz/launch/launch_config/solo.yaml"/> <rosparam file="/home/ivan/catkin_ws/src/rosbuzz/launch/launch_config/solo.yaml"/>
<param name="bzzfile_name" value="/home/ivan/catkin_ws/src/rosbuzz/script/testalone.bzz" /> <param name="bzzfile_name" value="/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz" />
<param name="rcclient" value="true" /> <param name="rcclient" value="true" />
<param name="rcservice_name" value="buzzcmd" /> <param name="rcservice_name" value="buzzcmd" />
<param name="in_payload" value="inMavlink"/> <param name="in_payload" value="inMavlink"/>

1560
script/testsolo.basm
View File

File diff suppressed because it is too large Load Diff

BIN
script/testsolo.bdb
View File

Binary file not shown.

BIN
script/testsolo.bdbg
View File

Binary file not shown.

BIN
script/testsolo.bo
View File

Binary file not shown.

5
script/testsolo.bzz
View File

@ -51,10 +51,9 @@ function hexagon() {
statef=land statef=land
} else { } else {
if(timeW >= (WAIT_TIMEOUT / 2)){ if(timeW >= (WAIT_TIMEOUT / 2)){
#uav_moveto(4.0,0.0) uav_moveto(0.03,0.0)
} else { } else {
#uav_moveto(4.0,0.0) uav_moveto(0.0,0.03)
#uav_moveto(0.0,4.0)
} }
timeW = timeW+1 timeW = timeW+1
} }

18
src/roscontroller.cpp
View File

@ -7,6 +7,11 @@ namespace rosbzz_node{
---------------*/ ---------------*/
roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv) roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv)
{ {
home[0]=0.0;home[1]=0.0;home[2]=0.0;
target[0]=0.0;target[1]=0.0;target[2]=0.0;
cur_pos[0]=0.0;cur_pos[1]=0.0;cur_pos[2]=0.0;
ROS_INFO("Buzz_node"); ROS_INFO("Buzz_node");
/*Obtain parameters from ros parameter server*/ /*Obtain parameters from ros parameter server*/
Rosparameters_get(n_c_priv); Rosparameters_get(n_c_priv);
@ -32,7 +37,10 @@ namespace rosbzz_node{
setpoint_counter = 0; setpoint_counter = 0;
fcu_timeout = TIMEOUT; fcu_timeout = TIMEOUT;
home[0]=0.0;home[1]=0.0;home[2]=0.0;
//ros::Duration(0.1).sleep();
} }
/*--------------------- /*---------------------
@ -115,7 +123,7 @@ namespace rosbzz_node{
timer_step+=1; timer_step+=1;
maintain_pos(timer_step); maintain_pos(timer_step);
std::cout<< "HOME: " << home[0] << ", " << home[1] <<std::endl; std::cout<< "HOME: " << home[0] << ", " << home[1];
} }
/* Destroy updater and Cleanup */ /* Destroy updater and Cleanup */
//update_routine(bcfname.c_str(), dbgfname.c_str(),1); //update_routine(bcfname.c_str(), dbgfname.c_str(),1);
@ -764,8 +772,10 @@ namespace rosbzz_node{
ROS_INFO("[%i] ROSBuzz LocalPos: %.7f, %.7f", robot_id, local_pos[0], local_pos[1]); ROS_INFO("[%i] ROSBuzz LocalPos: %.7f, %.7f", robot_id, local_pos[0], local_pos[1]);
/*prepare the goto publish message ATTENTION: ENU FRAME FOR MAVROS STANDARD (then converted to NED)*/ /*prepare the goto publish message ATTENTION: ENU FRAME FOR MAVROS STANDARD (then converted to NED)*/
moveMsg.pose.position.x = local_pos[1]+y; target[0]+=y;
moveMsg.pose.position.y = local_pos[0]+x; target[1]+=x;
moveMsg.pose.position.x = target[0];//local_pos[1]+y;
moveMsg.pose.position.y = target[1];
moveMsg.pose.position.z = z; moveMsg.pose.position.z = z;
moveMsg.pose.orientation.x = 0; moveMsg.pose.orientation.x = 0;