ns_prefix integrated in source files instead of launch files

grvcTeam · Jul 12, 2017 · 0891d07 · 0891d07
1 parent 657d1de
commit 0891d07
Show file tree

Hide file tree

Showing 9 changed files with 39 additions and 25 deletions.
diff --git a/px4_bringup/launch/spawn_robot.launch b/px4_bringup/launch/spawn_robot.launch
@@ -17,7 +17,7 @@ THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLI
     <arg name="target_ip" default="localhost"/>
     <arg name="own_ip" default="localhost"/>
     <arg name="type" default="xacro"/>
-    <arg name="nsprefix" default="uav_"/>
+    <arg name="ns_prefix" default="uav_"/>
 
     <group if="$(eval mode == 'sitl')">
         <!-- Spawn model in Gazebo -->
@@ -31,7 +31,7 @@ THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLI
     </group>
 
     <!-- Run mavros node -->
-    <node pkg="px4_bringup" type="run_mavros.py" name="run_mavros" ns="$(arg nsprefix)$(arg id)" output="screen"
-    args="-id=$(arg id) -mode=$(arg mode) -target_ip=$(arg target_ip) -own_ip=$(arg own_ip)"/>
+    <node pkg="px4_bringup" type="run_mavros.py" name="run_mavros_$(arg id)" output="screen"
+    args="-id=$(arg id) -mode=$(arg mode) -target_ip=$(arg target_ip) -own_ip=$(arg own_ip) -ns_prefix=$(arg ns_prefix)"/>
 
 </launch>
diff --git a/px4_bringup/scripts/run_mavros.py b/px4_bringup/scripts/run_mavros.py
@@ -3,6 +3,7 @@
 import argparse
 import utils
 import rospkg
+import rospy
 
 
 def main():
@@ -17,6 +18,8 @@ def main():
                         help='IP address of the device running px4, used to set proper fcu_url')
     parser.add_argument('-own_ip', type=str, default="localhost",
                         help='IP address of this device, used to set proper fcu_url')
+    parser.add_argument('-ns_prefix', type=str, default="uav_",
+                        help='namespace prefix')
     args, unknown = parser.parse_known_args()
     utils.check_unknown_args(unknown)
 
@@ -28,14 +31,20 @@ def main():
     subprocess.call("mkdir -p " + temp_dir, shell=True)
 
     # Set a param to tell the system current spawn mode
-    run_ns = "run_mavros/uav_" + str(args.id)
+    run_ns = "run_mavros/" + args.ns_prefix + str(args.id)
     subprocess.call("rosparam set " + run_ns + "/mode " + args.mode, shell=True)
 
+    # Namespace
+    if rospy.get_namespace()=="/":
+        ns = args.ns_prefix + str(args.id)
+    else:
+        ns = rospy.get_namespace() + "/" + args.ns_prefix + str(args.id)
+
     # Get udp configuration, depending on id
     udp_config = utils.udp_config(args.id)
 
     # Set params for mavros...
-    node_name = "mavros"
+    node_name = ns + "/mavros"
     if args.mode == "sitl":
         fcu_url = "udp://:" + str(udp_config["o_port"][1]) + \
         "@127.0.0.1:" + str(udp_config["u_port"][1])
@@ -59,7 +68,7 @@ def main():
     node_name, shell=True)
 
     # Finally rosrun mavros
-    rosrun_args = "rosrun mavros mavros_node __name:=" + node_name
+    rosrun_args = "rosrun mavros mavros_node __name:=" + "mavros" + " __ns:=" + ns
     rosrun_out = open(temp_dir+"/mavros.out", 'w')
     rosrun_err = open(temp_dir+"/mavros.err", 'w')
     try:

diff --git a/px4_bringup/scripts/spawn_gzmodel.py b/px4_bringup/scripts/spawn_gzmodel.py
@@ -103,8 +103,8 @@ def main():
     tf_buffer = tf2_ros.Buffer(rospy.Duration(1200.0)) #tf buffer length
     tf_listener = tf2_ros.TransformListener(tf_buffer)
 
-    if rospy.has_param( '/uav_{}_home_frame'.format(args.id) ):
-        uav_frame = rospy.get_param( '/uav_{}_home_frame'.format(args.id) )
+    if rospy.has_param( 'uav_{}_home_frame'.format(args.id) ):
+        uav_frame = rospy.get_param( 'uav_{}_home_frame'.format(args.id) )
         if uav_frame['parent_frame']=='map':
             robot_home = uav_frame['translation']
             robot_yaw = uav_frame['rotation'][2]

diff --git a/uav_abstraction_layer/include/uav_abstraction_layer/ual.h b/uav_abstraction_layer/include/uav_abstraction_layer/ual.h
@@ -90,6 +90,7 @@ class UAL {
     std::atomic<State> state_ = {LANDED};
 
     int robot_id_;
+    std::string ns_prefix_;
 };
 
 }}	// namespace grvc::ual

diff --git a/uav_abstraction_layer/launch/test_light_simulation.launch b/uav_abstraction_layer/launch/test_light_simulation.launch
@@ -12,7 +12,7 @@ THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLI
 
     <arg name="multi" default="false"/>
     <arg name="backend" default="light"/>
-    <arg name="nsprefix" default="uav_"/>
+    <arg name="ns_prefix" default="uav_"/>
 
     <!-- Load frame transforms for map and UAVs -->
     <rosparam file="$(find px4_bringup)/config/simulation_params.yaml" command="load" />
@@ -38,13 +38,16 @@ THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLI
     </group>
 
     <!-- Launch UAL server for uav id=1 -->
-    <node pkg="uav_abstraction_layer" type="server" name="server" output="screen" ns="$(arg nsprefix)1" args="-uav_id=1 -backend=$(arg backend) -pose_frame_id=map"/>
+    <node pkg="uav_abstraction_layer" type="server" name="server_1" output="screen"
+        args="-uav_id=1 -ns_prefix=$(arg ns_prefix) -backend=$(arg backend) -pose_frame_id=map"/>
 
     <group if="$(arg multi)">
         <!-- Launch UAL server for uav id=2 -->
-        <node pkg="uav_abstraction_layer" type="server" name="server" output="screen" ns="$(arg nsprefix)2" args="-uav_id=2 -backend=$(arg backend) -pose_frame_id=map"/>
+        <node pkg="uav_abstraction_layer" type="server" name="server_2" output="screen"
+            args="-uav_id=2 -ns_prefix=$(arg ns_prefix) -backend=$(arg backend) -pose_frame_id=map"/>
         <!-- Launch UAL server for uav id=3 -->
-        <node pkg="uav_abstraction_layer" type="server" name="server" output="screen" ns="$(arg nsprefix)3" args="-uav_id=3 -backend=$(arg backend) -pose_frame_id=map"/>
+        <node pkg="uav_abstraction_layer" type="server" name="server_3" output="screen"
+            args="-uav_id=3 -ns_prefix=$(arg ns_prefix) -backend=$(arg backend) -pose_frame_id=map"/>
     </group>
 
 </launch>
diff --git a/uav_abstraction_layer/launch/test_server.launch b/uav_abstraction_layer/launch/test_server.launch
@@ -11,7 +11,7 @@ THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLI
 <launch>
 
     <arg name="multi" default="false"/>
-    <arg name="nsprefix" default="uav_"/>
+    <arg name="ns_prefix" default="uav_"/>
 
     <!-- Load frame transforms for map and UAVs -->
     <rosparam file="$(find px4_bringup)/config/simulation_params.yaml" command="load" />
@@ -33,7 +33,7 @@ THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLI
         <arg name="id" value="1"/>
         <arg name="material" value="Orange"/>
         <arg name="mode" value="sitl"/>
-        <arg name="nsprefix" value="$(arg nsprefix)"/>
+        <arg name="ns_prefix" value="$(arg ns_prefix)"/>
     </include>
 
     <group if="$(arg multi)">
@@ -42,25 +42,25 @@ THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLI
             <arg name="id" value="2"/>
             <arg name="material" value="Indigo"/>
             <arg name="mode" value="sitl"/>
-            <arg name="nsprefix" value="$(arg nsprefix)"/>
+            <arg name="ns_prefix" value="$(arg ns_prefix)"/>
         </include>
         <!-- Launch robot id=3 -->
         <include file="$(find px4_bringup)/launch/spawn_robot.launch">
             <arg name="id" value="3"/>
             <arg name="material" value="ZincYellow"/>
             <arg name="mode" value="sitl"/>
-            <arg name="nsprefix" value="$(arg nsprefix)"/>
+            <arg name="ns_prefix" value="$(arg ns_prefix)"/>
         </include>
     </group>
 
     <!-- Launch UAL server for uav id=1 -->
-    <node pkg="uav_abstraction_layer" type="server" name="server" output="screen" ns="$(arg nsprefix)1" args="-uav_id=1"/>
+    <node pkg="uav_abstraction_layer" type="server" name="server_1" output="screen" args="-uav_id=1 -ns_prefix=$(arg ns_prefix)"/>
 
     <group if="$(arg multi)">
         <!-- Launch UAL server for uav id=2 -->
-        <node pkg="uav_abstraction_layer" type="server" name="server" output="screen" ns="$(arg nsprefix)2" args="-uav_id=2"/>
+        <node pkg="uav_abstraction_layer" type="server" name="server_2" output="screen" args="-uav_id=2 -ns_prefix=$(arg ns_prefix)"/>
         <!-- Launch UAL server for uav id=3 -->
-        <node pkg="uav_abstraction_layer" type="server" name="server" output="screen" ns="$(arg nsprefix)3" args="-uav_id=3"/>
+        <node pkg="uav_abstraction_layer" type="server" name="server_3" output="screen" args="-uav_id=3 -ns_prefix=$(arg ns_prefix)"/>
     </group>
 
 </launch>
diff --git a/uav_abstraction_layer/src/backend_light.cpp b/uav_abstraction_layer/src/backend_light.cpp
@@ -50,8 +50,8 @@ BackendLight::BackendLight(grvc::utils::ArgumentParser& _args)
     // Parse arguments
     robot_id_ = _args.getArgument("uav_id", 1);
     pose_frame_id_ = _args.getArgument<std::string>("pose_frame_id", "");
-    max_h_vel_ = _args.getArgument("max_h_vel", 0.8); // m/s
-    max_v_vel_ = _args.getArgument("max_v_vel", 0.5); // m/s
+    max_h_vel_ = _args.getArgument("max_h_vel", 1.5); // m/s
+    max_v_vel_ = _args.getArgument("max_v_vel", 1.0); // m/s
     max_yaw_vel_ = _args.getArgument("max_yaw_vel", 0.2); // rad/s
 
     // Init ros communications
@@ -341,7 +341,7 @@ void BackendLight::initHomeFrame() {
     std::vector<double> rotation;
     std::string uav_home_text;
 
-    uav_home_text = "/" + uav_home_frame_id_ + "_frame";
+    uav_home_text = uav_home_frame_id_ + "_frame";
 
     if ( ros::param::has(uav_home_text) ) {
         ros::param::get(uav_home_text + "/frame_id", frame_id);

diff --git a/uav_abstraction_layer/src/backend_mavros.cpp b/uav_abstraction_layer/src/backend_mavros.cpp
@@ -43,7 +43,7 @@ BackendMavros::BackendMavros(grvc::utils::ArgumentParser& _args)
 
     // Init ros communications
     ros::NodeHandle nh;
-    std::string mavros_ns = "mavros";
+    std::string mavros_ns = _args.getArgument<std::string>("ns_prefix", "uav_") + std::to_string(this->robot_id_) + "/mavros";
     std::string set_mode_srv = mavros_ns + "/set_mode";
     std::string arming_srv = mavros_ns + "/cmd/arming";
     std::string set_pose_topic = mavros_ns + "/setpoint_position/local";
@@ -336,7 +336,7 @@ void BackendMavros::initHomeFrame() {
     std::vector<double> rotation;
     std::string uav_home_text;
 
-    uav_home_text = "/" + uav_home_frame_id_ + "_frame";
+    uav_home_text = uav_home_frame_id_ + "_frame";
 
     if ( ros::param::has(uav_home_text) ) {
         ros::param::get(uav_home_text + "/frame_id", frame_id);

diff --git a/uav_abstraction_layer/src/ual.cpp b/uav_abstraction_layer/src/ual.cpp
@@ -30,13 +30,14 @@ UAL::UAL(grvc::utils::ArgumentParser& _args) {
     // Create backend first of all, inits ros node
     backend_ = Backend::createBackend(_args);
     robot_id_ = _args.getArgument("uav_id", 1);
+    ns_prefix_ = _args.getArgument<std::string>("ns_prefix", "uav_");
 
     // Start server if explicitly asked
     std::string server_mode = _args.getArgument<std::string>("ual_server", "off");
     // TODO: Consider other modes?
     if (server_mode == "on") {
         server_thread_ = std::thread([this]() {
-            std::string ual_ns = "ual";//_" + std::to_string(this->robot_id_);
+            std::string ual_ns =  this->ns_prefix_ + std::to_string(this->robot_id_) + "/ual";//_" + std::to_string(this->robot_id_);
             std::string take_off_srv = ual_ns + "/take_off";
             std::string land_srv = ual_ns + "/land";
             std::string go_to_waypoint_srv = ual_ns + "/go_to_waypoint";