Add goal type

config/goals.yaml

@@ -3,13 +3,16 @@ goals:
    x: 1.0
    y: 0.22
    yaw: 0.0
+   type: 0
 
  - id: goal2
    x: 2.0
    y: 0.22
    yaw: 0.0
+   type: 1
 
  - id: goal3
    x: 3.0
    y: -0.22
    yaw: 3.14159
+   type: 2

launch/test.launch

 <?xml version="1.0" encoding="UTF-8"?>
 <launch>
-
+  
+  <arg name="goals_file"        default="$(find iri_adc_jury)/config/goals.yaml"/>
   <arg name="output"        default="screen"/>
   <arg name="launch_prefix" default=""/>
   <arg name="dr"            default="true"/>
 
-  <rosparam command="load" file="$(find iri_adc_jury)/config/goals.yaml" />
+  <rosparam command="load" file="$(arg goals_file)" />
 
   <include file="$(find iri_adc_jury)/launch/node.launch">
     <arg name="node_name"     value="iri_adc_jury"/>

src/adc_jury_alg_node.cpp

@@ -37,16 +37,18 @@ AdcJuryAlgNode::AdcJuryAlgNode(void) :
     for (int i = 0; i < goals.size(); i++)
     {
       XmlRpc::XmlRpcValue goal = goals[i];
-      std::string id=goal["id"];
-      double x = goal["x"];
-      double y = goal["y"];
-      double yaw = goal["yaw"];
-      ROS_INFO("AdcJuryAlgNode: Goal #%d with id,x,y,yaw=(%s,%f,%f,%f)",i, id.c_str(), x, y, yaw);
+      std::string id = goal["id"];
+      double x       = goal["x"];
+      double y       = goal["y"];
+      double yaw     = goal["yaw"];
+      int type       = goal["type"];
+      ROS_INFO("AdcJuryAlgNode: Goal #%d with id,x,y,yaw,type=(%s,%f,%f,%f,%d)",i, id.c_str(), x, y, yaw, type);
       iri_adc_msgs::adc_goal g;
       g.id=id;
       g.x=x;
       g.y=y;
       g.yaw=yaw;
+      g.type=type;
       adc_jury_goal_.goals.goals.push_back(g);
     }
   }