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

fix for m100

This commit is contained in:
David St-Onge 2017-02-17 20:03:09 -05:00
parent dc89d37c22
commit 2b76c3cc6b
6 changed files with 13 additions and 122 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
launch/launch_config/m100.yaml
View File

@ -2,7 +2,8 @@ topics:
gps : /global_position
battery : /power_status
status : /flight_status
fcclient : /dji_mavcmd
type:
gps : sensor_msgs/NavSatFix
battery : mavros_msgs/BatteryStatus
status : mavros_msgs/ExtendedState
status : mavros_msgs/ExtendedState

1
launch/launch_config/solo.yaml
View File

@ -2,6 +2,7 @@ topics:
gps : /mavros/global_position/global
battery : /mavros/battery
status : /mavros/state
fcclient: /mavros/cmd/command
type:
gps : sensor_msgs/NavSatFix
# for SITL Solo

2
launch/rosbuzz.launch
View File

@ -6,8 +6,6 @@
<param name="bzzfile_name" value="$(env ROS_WS)/src/ROSBuzz/src/test1.bzz" />
<param name="rcclient" value="true" />
<param name="rcservice_name" value="/buzzcmd" />
<!-- todo: verify if this will work, or use "command" -->
<param name="fcclient_name" value="/mavros/cmd/command" />
<param name="in_payload" value="/inMavlink"/>
<param name="out_payload" value="/outMavlink"/>
<param name="robot_id" value="3"/>

20
src/roscontroller.cpp
View File

@ -125,11 +125,8 @@ namespace rosbzz_node{
else if(rcclient==false){ROS_INFO("RC service is disabled");}
}
else{ROS_ERROR("Provide a rc client option: yes or no in Launch file"); system("rosnode kill rosbuzz_node");}
/*Obtain fc client name from parameter server*/
if(n_c.getParam("fcclient_name", fcclient_name));
else {ROS_ERROR("Provide a fc client name in Launch file"); system("rosnode kill rosbuzz_node");}
/*Obtain robot_id from parameter server*/
n_c.getParam("robot_id", robot_id);
n_c.getParam("robot_id", robot_id);
/*Obtain out payload name*/
n_c.getParam("out_payload", out_payload);
/*Obtain in payload name*/
@ -162,6 +159,10 @@ namespace rosbzz_node{
node_handle.getParam("topics/status", status_topic);
node_handle.getParam("type/status", status_type);
m_smTopic_infos.insert(pair <std::string, std::string>(status_topic, status_type));
/*Obtain fc client name from parameter server*/
if(node_handle.getParam("topics/fcclient", fcclient_name));
else {ROS_ERROR("Provide a fc client name in Launch file"); system("rosnode kill rosbuzz_node");}
}
void roscontroller::Initialize_pub_sub(ros::NodeHandle n_c){
@ -256,8 +257,8 @@ namespace rosbzz_node{
else ROS_ERROR("Failed to call service from flight controller");
} else if (tmp == 2) { /*FC call for goto*/
/*prepare the goto publish message */
cmd_srv.request.param5 = goto_pos[0]*pow(10,7);
cmd_srv.request.param6 = goto_pos[1]*pow(10,7);
cmd_srv.request.param5 = goto_pos[0];
cmd_srv.request.param6 = goto_pos[1];
cmd_srv.request.param7 = goto_pos[2];
cmd_srv.request.command = buzzuav_closures::getcmd();
if (mav_client.call(cmd_srv)){ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); }
@ -685,11 +686,8 @@ namespace rosbzz_node{
case mavros_msgs::CommandCode::NAV_WAYPOINT:
ROS_INFO("RC_Call: GO TO!!!! ");
double rc_goto[3];
//rc_goto[0]=req.x/pow(10,7);
//rc_goto[1]=req.y/pow(10,7);
//rc_goto[2]=req.z;
rc_goto[0] = req.param5 / pow(10, 7);
rc_goto[1] = req.param6 / pow(10, 7);
rc_goto[0] = req.param5;
rc_goto[1] = req.param6;
rc_goto[2] = req.param7;
buzzuav_closures::rc_set_goto(rc_goto);

2
src/test1.bzz
View File

@ -1,7 +1,7 @@
# We need this for 2D vectors
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
include "/home/ivan/dev/buzz/src/include/vec2.bzz"
include "/home/ubuntu/buzz/src/include/vec2.bzz"
####################################################################################################
# Updater related
# This should be here for the updater to work, changing position of code will crash the updater

107
src/vec2.bzz
View File

@ -1,107 +0,0 @@
#
# Create a new namespace for vector2 functions
#
math.vec2 = {}
#
# Creates a new vector2.
# PARAM x: The x coordinate.
# PARAM y: The y coordinate.
# RETURN: A new vector2.
#
math.vec2.new = function(x, y) {
return { .x = x, .y = y }
}
#
# Creates a new vector2 from polar coordinates.
# PARAM l: The length of the vector2.
# PARAM a: The angle of the vector2.
# RETURN: A new vector2.
#
math.vec2.newp = function(l, a) {
return {
.x = l * math.cos(a),
.y = l * math.sin(a)
}
}
#
# Calculates the length of the given vector2.
# PARAM v: The vector2.
# RETURN: The length of the vector.
#
math.vec2.length = function(v) {
return math.sqrt(v.x * v.x + v.y * v.y)
}
#
# Calculates the angle of the given vector2.
# PARAM v: The vector2.
# RETURN: The angle of the vector.
#
math.vec2.angle = function(v) {
return math.atan2(v.y, v.x)
}
#
# Returns the normalized form of a vector2.
# PARAM v: The vector2.
# RETURN: The normalized form.
#
math.vec2.norm = function(v) {
var l = math.length(v)
return {
.x = v.x / l,
.y = v.y / l
}
}
#
# Calculates v1 + v2.
# PARAM v1: A vector2.
# PARAM v2: A vector2.
# RETURN: v1 + v2
#
math.vec2.add = function(v1, v2) {
return {
.x = v1.x + v2.x,
.y = v1.y + v2.y
}
}
#
# Calculates v1 - v2.
# PARAM v1: A vector2.
# PARAM v2: A vector2.
# RETURN: v1 + v2
#
math.vec2.sub = function(v1, v2) {
return {
.x = v1.x - v2.x,
.y = v1.y - v2.y
}
}
#
# Scales a vector by a numeric constant.
# PARAM v: A vector2.
# PARAM s: A number (float or int).
# RETURN: s * v
#
math.vec2.scale = function(v, s) {
return {
.x = v.x * s,
.y = v.y * s
}
}
#
# Calculates v1 . v2 (the dot product)
# PARAM v1: A vector2.
# PARAM v2: A vector2.
# RETURN: v1 . v2
#
math.vec2.dot = function(v1, v2) {
return v1.x * v2.x + v1.y * v2.y
}