Kaynağa Gözat

添加launch文件,修改感知模块无法启动bug

madesheng 4 yıl önce
ebeveyn
işleme
a7165ea81c

+ 14 - 0
src/launch/robot.launch

@@ -0,0 +1,14 @@
+<launch>
+<!--name="NODE_NAME"为节点指派名称,这将会覆盖掉ros::init()定义的node_name;
+pkg="PACKAGE_NAME"节点所在的包名
+type="FILE_NAME" 执行文件的名称如果是用Python编写的就填写xxx.py,如果是cpp就写编译生成的可执行文件名
+output="screen"终端输出转储在当前的控制台上,而不是在日志文件中
+respawn="true"当roslaunch启动完所有该启动的节点之后,会监测每一个节点,保证它们正常的运行状态。对于任意节点,当它终止时,roslaunch 会将该节点重启
+required="true"当被此属性标记的节点终止时,roslaunch会将其他的节点一并终止。注意此属性不可以与respawn="true"一起描述同一个节点
+launch-prefix = "command-prefix"我的理解是,相当于在执行启动命令时加上一段命令前缀
+ns = "NAME_SPACE" 这个属性可以让你在自定义的命名空间里运行节点-->
+<node pkg="decision" name="decision" type="decision" output="screen" required="true"/>
+<node pkg="perception" name="perception" type="perception" output="screen" respawn="true"/>
+<node pkg="localization" name="localization" type="localization" output="screen" respawn="true"/>
+<node pkg="canbus" name="canbus" type="canbus" output="screen" respawn="true"/>
+</launch>

+ 2 - 0
src/localization/localization.cpp

@@ -107,7 +107,9 @@ int Localization::GetSerialData()
 }
 
 void Localization::Stop() {
+  LOG(INFO)<<Name()<<" Stop start...";
   serial_gnss.close();
+  LOG(INFO)<<Name()<<" Stop end.";
 }
 
 

+ 10 - 4
src/perception/perception.cpp

@@ -20,24 +20,30 @@ int Perception::Init()
     node_handle_.reset(new ros::NodeHandle());
 
     LOG(INFO) << Name() << " Init end.";
+    return ROBOT_SUCCESS;
 }
 
 int Perception::Start()
 {
+    LOG(INFO) << Name() << " Start start...";
     try
     {
         ros::Subscriber sub = node_handle_->subscribe(robot::common::CanDataAnalysis, 1000, &Perception::CanTopicCallback, this);
+        ros::spin();
     }
     catch(ros::Exception e)
     {
-         LOG(INFO) << Name() << " Start exception is:" << e.what() ;
-    }
-    ros::spin();
+         LOG(INFO) << Name() << " Start exception is:" << e.what();
+         return ROBOT_FAILTURE;
+    }  
+     LOG(INFO) << Name() << " Start end.";
+    return ROBOT_SUCCESS;
 }
 
 void Perception::Stop()
 {
-
+   LOG(INFO) << Name() << " Stop start...";
+    LOG(INFO) << Name() << " Stop end.";
 }
 
 

+ 3 - 0
src/perception/perception.h

@@ -9,6 +9,9 @@
 namespace robot {
 namespace perception {
 
+#define ROBOT_SUCCESS     0
+#define ROBOT_FAILTURE   -1
+
 class Perception : public robot::common::RobotApp
 {
 public: