added launch_config arg and made BVM state optional on dev
This commit is contained in:
parent
a9038601ae
commit
4845ff262d
@ -98,6 +98,7 @@ private:
|
|||||||
int armstate;
|
int armstate;
|
||||||
int barrier;
|
int barrier;
|
||||||
int update;
|
int update;
|
||||||
|
int statepub_active;
|
||||||
int message_number = 0;
|
int message_number = 0;
|
||||||
uint8_t no_of_robots = 0;
|
uint8_t no_of_robots = 0;
|
||||||
bool rcclient;
|
bool rcclient;
|
||||||
|
@ -6,9 +6,10 @@
|
|||||||
<arg name="name" default="robot0"/>
|
<arg name="name" default="robot0"/>
|
||||||
<arg name="xbee_plugged" default="true"/>
|
<arg name="xbee_plugged" default="true"/>
|
||||||
<arg name="script" default="testalone"/>
|
<arg name="script" default="testalone"/>
|
||||||
|
<arg name="launch_config" default="topics"/>
|
||||||
|
|
||||||
<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="$(find rosbuzz)/launch/launch_config/topics.yaml"/>
|
<rosparam file="$(find rosbuzz)/launch/launch_config/$(arg launch_config).yaml"/>
|
||||||
<param name="bzzfile_name" value="$(find rosbuzz)/buzz_scripts/$(arg script).bzz" />
|
<param name="bzzfile_name" value="$(find rosbuzz)/buzz_scripts/$(arg script).bzz" />
|
||||||
<param name="rcclient" value="true" />
|
<param name="rcclient" value="true" />
|
||||||
<param name="rcservice_name" value="buzzcmd" />
|
<param name="rcservice_name" value="buzzcmd" />
|
||||||
|
@ -6,9 +6,10 @@
|
|||||||
<arg name="name" default="robot0"/>
|
<arg name="name" default="robot0"/>
|
||||||
<arg name="xbee_plugged" default="true"/>
|
<arg name="xbee_plugged" default="true"/>
|
||||||
<arg name="script" default="testalone"/>
|
<arg name="script" default="testalone"/>
|
||||||
|
<arg name="launch_config" default="topics"/>
|
||||||
|
|
||||||
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" launch-prefix="gdb -ex run --args">
|
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" launch-prefix="gdb -ex run --args">
|
||||||
<rosparam file="$(find rosbuzz)/launch/launch_config/topics.yaml"/>
|
<rosparam file="$(find rosbuzz)/launch/launch_config/$(arg launch_config).yaml"/>
|
||||||
<param name="bzzfile_name" value="$(find rosbuzz)/buzz_scripts/$(arg script).bzz" />
|
<param name="bzzfile_name" value="$(find rosbuzz)/buzz_scripts/$(arg script).bzz" />
|
||||||
<param name="rcclient" value="true" />
|
<param name="rcclient" value="true" />
|
||||||
<param name="rcservice_name" value="buzzcmd" />
|
<param name="rcservice_name" value="buzzcmd" />
|
||||||
|
@ -110,13 +110,24 @@ void roscontroller::RosControllerRun()
|
|||||||
update = buzz_update::init_update_monitor(bcfname.c_str(), standby_bo.c_str(), dbgfname.c_str(), robot_id);
|
update = buzz_update::init_update_monitor(bcfname.c_str(), standby_bo.c_str(), dbgfname.c_str(), robot_id);
|
||||||
// set ROS loop rate
|
// set ROS loop rate
|
||||||
ros::Rate loop_rate(BUZZRATE);
|
ros::Rate loop_rate(BUZZRATE);
|
||||||
|
// check for BVMSTATE variable
|
||||||
|
buzzvm_t VM = buzz_utility::get_vm();
|
||||||
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1));
|
||||||
|
buzzvm_gload(VM);
|
||||||
|
buzzobj_t obj = buzzvm_stack_at(VM, 1);
|
||||||
|
if(obj->o.type == BUZZTYPE_STRING) statepub_active = 1;
|
||||||
|
else
|
||||||
|
{
|
||||||
|
statepub_active = 0;
|
||||||
|
ROS_ERROR("BVMSTATE undeclared in .bzz file, BVMSTATE pusblisher disabled.");
|
||||||
|
}
|
||||||
// DEBUG
|
// DEBUG
|
||||||
// ROS_WARN("[%i] -----------------------STARTING MAIN LOOP!", robot_id);
|
// ROS_WARN("[%i] -----------------------STARTING MAIN LOOP!", robot_id);
|
||||||
while (ros::ok() && !buzz_utility::buzz_script_done())
|
while (ros::ok() && !buzz_utility::buzz_script_done())
|
||||||
{
|
{
|
||||||
// Publish topics
|
// Publish topics
|
||||||
neighbours_pos_publisher();
|
neighbours_pos_publisher();
|
||||||
uavstate_publisher();
|
if(statepub_active) uavstate_publisher();
|
||||||
grid_publisher();
|
grid_publisher();
|
||||||
send_MPpayload();
|
send_MPpayload();
|
||||||
// Check updater state and step code
|
// Check updater state and step code
|
||||||
@ -652,88 +663,88 @@ script
|
|||||||
armstate = 0;
|
armstate = 0;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case buzzuav_closures::COMMAND_GOHOME: // TODO: NOT FULLY IMPLEMENTED/TESTED !!!
|
// case buzzuav_closures::COMMAND_GOHOME: // TODO: NOT FULLY IMPLEMENTED/TESTED !!!
|
||||||
cmd_srv.request.param5 = home.latitude;
|
// cmd_srv.request.param5 = home.latitude;
|
||||||
cmd_srv.request.param6 = home.longitude;
|
// cmd_srv.request.param6 = home.longitude;
|
||||||
cmd_srv.request.param7 = home.altitude;
|
// cmd_srv.request.param7 = home.altitude;
|
||||||
cmd_srv.request.command = buzzuav_closures::getcmd();
|
// cmd_srv.request.command = buzzuav_closures::getcmd();
|
||||||
if (mav_client.call(cmd_srv))
|
// if (mav_client.call(cmd_srv))
|
||||||
{
|
// {
|
||||||
ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
|
// ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
|
||||||
}
|
// }
|
||||||
else
|
// else
|
||||||
ROS_ERROR("Failed to call service from flight controller");
|
// ROS_ERROR("Failed to call service from flight controller");
|
||||||
break;
|
// break;
|
||||||
|
|
||||||
case buzzuav_closures::COMMAND_GOTO: // TOOD: NOT FULLY IMPLEMENTED/TESTED !!!
|
// case buzzuav_closures::COMMAND_GOTO: // TOOD: NOT FULLY IMPLEMENTED/TESTED !!!
|
||||||
goto_pos = buzzuav_closures::getgoto();
|
// goto_pos = buzzuav_closures::getgoto();
|
||||||
cmd_srv.request.param5 = goto_pos[0];
|
// cmd_srv.request.param5 = goto_pos[0];
|
||||||
cmd_srv.request.param6 = goto_pos[1];
|
// cmd_srv.request.param6 = goto_pos[1];
|
||||||
cmd_srv.request.param7 = goto_pos[2];
|
// cmd_srv.request.param7 = goto_pos[2];
|
||||||
cmd_srv.request.command = buzzuav_closures::getcmd();
|
// cmd_srv.request.command = buzzuav_closures::getcmd();
|
||||||
if (mav_client.call(cmd_srv))
|
// if (mav_client.call(cmd_srv))
|
||||||
{
|
// {
|
||||||
ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
|
// ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
|
||||||
}
|
// }
|
||||||
else
|
// else
|
||||||
ROS_ERROR("Failed to call service from flight controller");
|
// ROS_ERROR("Failed to call service from flight controller");
|
||||||
cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START;
|
// cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START;
|
||||||
if (mav_client.call(cmd_srv))
|
// if (mav_client.call(cmd_srv))
|
||||||
{
|
// {
|
||||||
ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
|
// ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
|
||||||
}
|
// }
|
||||||
else
|
// else
|
||||||
ROS_ERROR("Failed to call service from flight controller");
|
// ROS_ERROR("Failed to call service from flight controller");
|
||||||
break;
|
// break;
|
||||||
|
|
||||||
case buzzuav_closures::COMMAND_ARM:
|
// case buzzuav_closures::COMMAND_ARM:
|
||||||
if (!armstate)
|
// if (!armstate)
|
||||||
{
|
// {
|
||||||
SetMode("LOITER", 0);
|
// SetMode("LOITER", 0);
|
||||||
armstate = 1;
|
// armstate = 1;
|
||||||
Arm();
|
// Arm();
|
||||||
}
|
// }
|
||||||
break;
|
// break;
|
||||||
|
|
||||||
case buzzuav_closures::COMMAND_DISARM:
|
// case buzzuav_closures::COMMAND_DISARM:
|
||||||
if (armstate)
|
// if (armstate)
|
||||||
{
|
// {
|
||||||
armstate = 0;
|
// armstate = 0;
|
||||||
SetMode("LOITER", 0);
|
// SetMode("LOITER", 0);
|
||||||
Arm();
|
// Arm();
|
||||||
}
|
// }
|
||||||
break;
|
// break;
|
||||||
|
|
||||||
case buzzuav_closures::COMMAND_MOVETO:
|
// case buzzuav_closures::COMMAND_MOVETO:
|
||||||
goto_pos = buzzuav_closures::getgoto();
|
// goto_pos = buzzuav_closures::getgoto();
|
||||||
roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], goto_pos[3]);
|
// roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], goto_pos[3]);
|
||||||
break;
|
// break;
|
||||||
|
|
||||||
case buzzuav_closures::COMMAND_GIMBAL:
|
// case buzzuav_closures::COMMAND_GIMBAL:
|
||||||
gimbal = buzzuav_closures::getgimbal();
|
// gimbal = buzzuav_closures::getgimbal();
|
||||||
cmd_srv.request.param1 = gimbal[0];
|
// cmd_srv.request.param1 = gimbal[0];
|
||||||
cmd_srv.request.param2 = gimbal[1];
|
// cmd_srv.request.param2 = gimbal[1];
|
||||||
cmd_srv.request.param3 = gimbal[2];
|
// cmd_srv.request.param3 = gimbal[2];
|
||||||
cmd_srv.request.param4 = gimbal[3];
|
// cmd_srv.request.param4 = gimbal[3];
|
||||||
cmd_srv.request.command = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL;
|
// cmd_srv.request.command = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL;
|
||||||
if (mav_client.call(cmd_srv))
|
// if (mav_client.call(cmd_srv))
|
||||||
{
|
// {
|
||||||
ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
|
// ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
|
||||||
}
|
// }
|
||||||
else
|
// else
|
||||||
ROS_ERROR("Failed to call service from flight controller");
|
// ROS_ERROR("Failed to call service from flight controller");
|
||||||
break;
|
// break;
|
||||||
|
|
||||||
case buzzuav_closures::COMMAND_PICTURE:
|
// case buzzuav_closures::COMMAND_PICTURE:
|
||||||
ROS_INFO("TAKING A PICTURE HERE!! --------------");
|
// ROS_INFO("TAKING A PICTURE HERE!! --------------");
|
||||||
mavros_msgs::CommandBool capture_command;
|
// mavros_msgs::CommandBool capture_command;
|
||||||
if (capture_srv.call(capture_command))
|
// if (capture_srv.call(capture_command))
|
||||||
{
|
// {
|
||||||
ROS_INFO("Reply: %ld", (long int)capture_command.response.success);
|
// ROS_INFO("Reply: %ld", (long int)capture_command.response.success);
|
||||||
}
|
// }
|
||||||
else
|
// else
|
||||||
ROS_ERROR("Failed to call service from camera streamer");
|
// ROS_ERROR("Failed to call service from camera streamer");
|
||||||
break;
|
// break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user