为机器人编队控制单独新增一个robot_bringup_team.launch启动文件

src/control_robot/control_robot.py

@@ -1,7 +1,7 @@
 #!/usr/bin/env python
 # coding:utf-8
 
-# Copyright: 2016-2022 wwww.corvin.cn ROS小课堂
+# Copyright: 2016-2024 wwww.corvin.cn ROS小课堂
 # Description: 通过键盘遥控小车行进，摄像头舵机转动,2DOF机械臂转动.
 # History:
 #    20200726:增加控制左右转动舵机的代码.
@@ -86,8 +86,9 @@ def angles(angle):
 
 #调用service: mobilebase_arduino/servo_write控制对应舵机转动
 def client_srv(ser_id, ser_angle):
-    robot_ns = os.environ.get('ROBOT_NS')
-    service_name = robot_ns + "/" + "mobilebase_arduino/servo_write"
+    #robot_ns = os.environ.get('ROBOT_NS')
+    #service_name = robot_ns + "/" + "mobilebase_arduino/servo_write"
+    service_name = "mobilebase_arduino/servo_write"
     rospy.wait_for_service(service_name)
     try:
         camera_client = rospy.ServiceProxy(service_name, ServoWrite)
@@ -97,9 +98,9 @@ def client_srv(ser_id, ser_angle):
 
 if __name__=="__main__":
     settings = termios.tcgetattr(sys.stdin)
-    robot_ns = os.environ.get('ROBOT_NS')
-    cmd_name = robot_ns + "/" + "cmd_vel"
-
+    #robot_ns = os.environ.get('ROBOT_NS')
+    #cmd_name = robot_ns + "/" + "cmd_vel"
+    cmd_name = "cmd_vel"
     pub = rospy.Publisher(cmd_name, Twist, queue_size = 1)
     rospy.init_node('control_robot_node')
 
@@ -188,4 +189,3 @@ if __name__=="__main__":
         pub.publish(twist)
 
         termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
-

src/robot_bringup/launch/robot_bringup.launch

@@ -13,68 +13,62 @@
     20220928:将robot_pose_ekf的数据融合发布频率从10hz提高为20hz,同时将输出的
       tf变换从odom_combined更新为odom,这样就不用在arduino轮式里程计那边发布odom的tf了.
     20230327:增加使用usb_cam节点的代码,当外接usb相机的时候可以使用该代码.
-    20240106:根据环境变量IS_USB_CAMERA来启动不同的摄像头,CSI或者USB类型摄像头.
+    20240113:根据环境变量IS_USB_CAMERA来启动不同的摄像头,CSI或者USB类型摄像头.
       需要在.bashrc文件中配置好该变量(export IS_USB_CAMERA=true/false).
-      配置节点运行命名空间,用于机器人编队.
 -->
 <launch>
-    <!-- 配置机器人的命名空间,用于多机器人编队 -->
-    <arg name="robot_ns" default="$(env ROBOT_NS)" />
-
     <!-- 根据环境变量配置的不同摄像头类型来启动对应的摄像头 -->
     <arg name="is_usb_camera" default="$(env IS_USB_CAMERA)" />
 
-    <group ns="$(arg robot_ns)">
-        <!-- (1) startup panda robot urdf description -->
-        <include file="$(find robot_description)/launch/robot_description.launch" />
+    <!-- (1) startup panda robot urdf description -->
+    <include file="$(find robot_description)/launch/robot_description.launch" />
 
-        <!-- (2) startup mobilebase arduino launch -->
-        <include file="$(find ros_arduino_python)/launch/arduino.launch" />
+    <!-- (2) startup mobilebase arduino launch -->
+    <include file="$(find ros_arduino_python)/launch/arduino.launch" />
 
-        <!-- (3) startup rasp serial imu-6DOF board-->
-        <include file="$(find serial_imu_hat_6dof)/launch/serial_imu_hat.launch" />
+    <!-- (3) startup rasp serial imu-6DOF board-->
+    <include file="$(find serial_imu_hat_6dof)/launch/serial_imu_hat.launch" />
 
-        <!-- (4) startup rasp cam -->
-        <group unless="$(arg is_usb_camera)">
-          <include file="$(find rasp_camera)/launch/rasp_camera.launch" />
-        </group>
+    <!-- (4) startup rasp cam -->
+    <group unless="$(arg is_usb_camera)">
+      <include file="$(find rasp_camera)/launch/rasp_camera.launch" />
+    </group>
 
-        <group if="$(arg is_usb_camera)">
-          <node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen" >
-            <param name="video_device"    value="/dev/video0" />
-            <param name="image_width"     value="640" />
-            <param name="image_height"    value="480" />
-            <param name="pixel_format"    value="yuyv" />
-            <param name="color_format"    value="yuv422p" />
-            <param name="camera_frame_id" value="usb_cam" />
-            <param name="io_method"       value="mmap"/>
-            <param name="framerate"       value="25" />
-            <param name="autofocus"       value="true" />
-          </node>
-        </group>
+    <group if="$(arg is_usb_camera)">
+      <node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen" >
+        <param name="video_device"    value="/dev/video0" />
+        <param name="image_width"     value="640" />
+        <param name="image_height"    value="480" />
+        <param name="pixel_format"    value="yuyv" />
+        <param name="color_format"    value="yuv422p" />
+        <param name="camera_frame_id" value="usb_cam" />
+        <param name="io_method"       value="mmap"/>
+        <param name="framerate"       value="25" />
+        <param name="autofocus"       value="true" />
+      </node>
+    </group>
 
-        <!-- (5) startup robot_pose_ekf node -->
-        <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf" output="screen">
-            <param name="output_frame" value="odom"/>
-            <param name="freq"         value="20.0"/>
-            <param name="odom_used"    value="true"/>
-            <param name="imu_used"     value="true"/>
-            <param name="vo_used"      value="false"/>
-            <param name="debug"        value="false"/>
-            <param name="sensor_timeout" value="1.0"/>
-            <param name="self_diagnose"  value="false"/>
-        </node>
+    <!-- (5) startup robot_pose_ekf node -->
+    <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf" output="screen">
+        <param name="output_frame" value="odom"/>
+        <param name="freq"         value="20.0"/>
+        <param name="odom_used"    value="true"/>
+        <param name="imu_used"     value="true"/>
+        <param name="vo_used"      value="false"/>
+        <param name="debug"        value="false"/>
+        <param name="sensor_timeout" value="1.0"/>
+        <param name="self_diagnose"  value="false"/>
+    </node>
 
-        <!-- (6) dwa_local_planner subscribe /odom_ekf topic -->
-        <node pkg="robot_bringup" type="odom_ekf.py" name="odom_ekf_node" output="screen">
-            <remap from="input_topic"  to="$(arg robot_ns)/robot_pose_ekf/odom_combined" />
-            <remap from="output_topic" to="$(arg robot_ns)/odom_ekf" />
-        </node>
+    <!-- (6) dwa_local_planner subscribe /odom_ekf topic -->
+    <node pkg="robot_bringup" type="odom_ekf.py" name="odom_ekf_node" output="screen">
+        <remap from="input_topic"  to="robot_pose_ekf/odom_combined" />
+        <remap from="output_topic" to="odom_ekf" />
+    </node>
 
-        <!-- (7) startup get battery oled show -->
-        <node pkg="robot_bringup" type="get_battery_node" name="get_battery_node" output="screen">
-          <param name="bat_topic" value="$(arg robot_ns)/mobilebase_arduino/sensor/batPercent" />
-        </node>
-    </group>
+    <!-- (7) startup get battery oled show -->
+    <node pkg="robot_bringup" type="get_battery_node" name="get_battery_node" output="screen">
+      <param name="bat_topic" value="mobilebase_arduino/sensor/batPercent" />
+    </node>
 </launch>
 

src/robot_bringup/launch/robot_bringup_team.launch

@@ -0,0 +1,80 @@
+<!--
+  Copyright: 2016-2024 ROS小课堂 www.corvin.cn
+  Description:robot启动时最先需要启动的launch文件,主要加载各基础功能模块启动.
+    首先就需要启动机器人的urdf文件,这样就可以在rviz中可视化显示机器人模型.然后
+    启动下位机arduino的程序,上下位机建立连接,这样上位机就可以发送控制命令.接下来
+    启动IMU模块,开始发布姿态信息.接下来就开始进行信息融合,使用imu信息和轮式里程计
+    这样得出的里程计更为准确,同时需要注意将融合后的tf更新为odom.
+  Author: corvin
+  History:
+    20200716:init this file.
+    20210602:使用串口获取imu模块的数据.
+    20220902:增加启动oled显示屏的launch.
+    20220928:将robot_pose_ekf的数据融合发布频率从10hz提高为20hz,同时将输出的
+      tf变换从odom_combined更新为odom,这样就不用在arduino轮式里程计那边发布odom的tf了.
+    20230327:增加使用usb_cam节点的代码,当外接usb相机的时候可以使用该代码.
+    20240106:根据环境变量IS_USB_CAMERA来启动不同的摄像头,CSI或者USB类型摄像头.
+      需要在.bashrc文件中配置好该变量(export IS_USB_CAMERA=true/false).
+      配置节点运行命名空间,用于机器人编队.
+-->
+<launch>
+    <!-- 配置机器人的命名空间,用于多机器人编队 -->
+    <arg name="robot_ns" default="$(env ROBOT_NS)" />
+
+    <!-- 根据环境变量配置的不同摄像头类型来启动对应的摄像头 -->
+    <arg name="is_usb_camera" default="$(env IS_USB_CAMERA)" />
+
+    <group ns="$(arg robot_ns)">
+        <!-- (1) startup panda robot urdf description -->
+        <include file="$(find robot_description)/launch/robot_description.launch" />
+
+        <!-- (2) startup mobilebase arduino launch -->
+        <include file="$(find ros_arduino_python)/launch/arduino.launch" />
+
+        <!-- (3) startup rasp serial imu-6DOF board-->
+        <include file="$(find serial_imu_hat_6dof)/launch/serial_imu_hat.launch" />
+
+        <!-- (4) startup rasp cam -->
+        <group unless="$(arg is_usb_camera)">
+          <include file="$(find rasp_camera)/launch/rasp_camera.launch" />
+        </group>
+
+        <group if="$(arg is_usb_camera)">
+          <node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen" >
+            <param name="video_device"    value="/dev/video0" />
+            <param name="image_width"     value="640" />
+            <param name="image_height"    value="480" />
+            <param name="pixel_format"    value="yuyv" />
+            <param name="color_format"    value="yuv422p" />
+            <param name="camera_frame_id" value="usb_cam" />
+            <param name="io_method"       value="mmap"/>
+            <param name="framerate"       value="25" />
+            <param name="autofocus"       value="true" />
+          </node>
+        </group>
+
+        <!-- (5) startup robot_pose_ekf node -->
+        <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf" output="screen">
+            <param name="output_frame" value="odom"/>
+            <param name="freq"         value="20.0"/>
+            <param name="odom_used"    value="true"/>
+            <param name="imu_used"     value="true"/>
+            <param name="vo_used"      value="false"/>
+            <param name="debug"        value="false"/>
+            <param name="sensor_timeout" value="1.0"/>
+            <param name="self_diagnose"  value="false"/>
+        </node>
+
+        <!-- (6) dwa_local_planner subscribe /odom_ekf topic -->
+        <node pkg="robot_bringup" type="odom_ekf.py" name="odom_ekf_node" output="screen">
+            <remap from="input_topic"  to="$(arg robot_ns)/robot_pose_ekf/odom_combined" />
+            <remap from="output_topic" to="$(arg robot_ns)/odom_ekf" />
+        </node>
+
+        <!-- (7) startup get battery oled show -->
+        <node pkg="robot_bringup" type="get_battery_node" name="get_battery_node" output="screen">
+          <param name="bat_topic" value="$(arg robot_ns)/mobilebase_arduino/sensor/batPercent" />
+        </node>
+    </group>
+</launch>
+