Commit 10174876 authored by xiaoqiang_user's avatar xiaoqiang_user

first version

parent cf8f2d04
<launch>
<group ns="backscan">
<node name="rplidarNode" pkg="rplidar_ros" type="rplidarNode" output="screen">
<param name="serial_port" type="string" value="/dev/ttyUSB022"/>
<param name="serial_baudrate" type="int" value="115200"/>
<param name="frame_id" type="string" value="laser2"/>
<param name="inverted" type="bool" value="false"/>
<param name="angle_compensate" type="bool" value="true"/>
</node>
</group>
<node pkg="tf" type="static_transform_publisher" name="laserlink_broadcaster2" args="-0.5 0 0.15 1.57080 0 0 base_link laser2 100"/>
</launch>
......@@ -9,8 +9,12 @@ import rospy
import roslaunch
import time
from galileo_serial_server.msg import GalileoStatus
import rosservice
import subprocess
import os
LAST_UPDATE_TIME = 0
LAST_UPDATE_TIME2 = 0
VISUAL_FLAG = False
......@@ -18,6 +22,9 @@ def get_scan(scan):
global LAST_UPDATE_TIME
LAST_UPDATE_TIME = int(time.time())
def get_scan2(scan):
global LAST_UPDATE_TIME2
LAST_UPDATE_TIME2 = int(time.time())
def get_galileo_status(status):
global VISUAL_FLAG
......@@ -30,6 +37,7 @@ def get_galileo_status(status):
if __name__ == "__main__":
rospy.init_node("rplidar_manager")
rospy.Subscriber("/scan", rospy.AnyMsg, get_scan)
rospy.Subscriber("/backscan/scan", rospy.AnyMsg, get_scan2)
rospy.Subscriber("/galileo/status", GalileoStatus, get_galileo_status)
rospy.set_param("~keep_running", True)
uuid = roslaunch.rlutil.get_or_generate_uuid(None, False)
......@@ -38,6 +46,12 @@ if __name__ == "__main__":
uuid, ["/home/xiaoqiang/Documents/ros/src/rplidar_ros/launch/rplidar.launch"])
rplidar_launch.start()
rospy.loginfo("rplidar started")
uuid2 = roslaunch.rlutil.get_or_generate_uuid(None, False)
roslaunch.configure_logging(uuid2)
rplidar_launch2 = roslaunch.parent.ROSLaunchParent(
uuid2, ["/home/xiaoqiang/Documents/ros/src/rplidar_ros/launch/rplidar2.launch"])
rplidar_launch2.start()
rospy.loginfo("rplidar2 started")
while not rospy.is_shutdown():
time.sleep(5)
keep_running_flag = rospy.get_param("~keep_running", True)
......@@ -50,5 +64,30 @@ if __name__ == "__main__":
uuid, ["/home/xiaoqiang/Documents/ros/src/rplidar_ros/launch/rplidar.launch"])
rplidar_launch.start()
# 在停用状态下却有雷达数据
if int(time.time()) - LAST_UPDATE_TIME < 5 and not keep_running_flag:
rplidar_launch.shutdown()
if int(time.time()) - LAST_UPDATE_TIME < 5 and not VISUAL_FLAG :
if rosservice.get_service_node("/stop_motor") is not None:
cmd = "rosservice call /stop_motor"
new_env = os.environ.copy()
subprocess.Popen(
cmd, shell=True, env=new_env)
time.sleep(5)
subprocess.Popen(
cmd, shell=True, env=new_env)
# 处于导航状态下且无法收到雷达数据
if int(time.time()) - LAST_UPDATE_TIME2 > 5 and VISUAL_FLAG and keep_running_flag:
rospy.logerr("restart rplidar2 node")
rplidar_launch2.shutdown()
roslaunch.configure_logging(uuid2)
rplidar_launch2 = roslaunch.parent.ROSLaunchParent(
uuid2, ["/home/xiaoqiang/Documents/ros/src/rplidar_ros/launch/rplidar2.launch"])
rplidar_launch2.start()
# 在停用状态下却有雷达数据
if int(time.time()) - LAST_UPDATE_TIME2 < 5 and not VISUAL_FLAG :
if rosservice.get_service_node("/backscan/stop_motor") is not None:
cmd = "rosservice call /backscan/stop_motor"
new_env = os.environ.copy()
subprocess.Popen(
cmd, shell=True, env=new_env)
time.sleep(5)
subprocess.Popen(
cmd, shell=True, env=new_env)
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment