go_to_position_2D_nav_goal

src/robot_behavior/CMakeLists.txt

@@ -195,6 +195,7 @@ catkin_install_python(PROGRAMS
          scripts/behavior_mpc_local_path_planner_action_server_main.py
          scripts/behavior_rl_local_path_planner_action_server_main.py
          scripts/behavior_social_cost_map_main.py
+         scripts/behavior_go_to_position_2D_nav_goal_main.py
                       DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 

src/robot_behavior/launch/main.launch

@@ -10,6 +10,7 @@
   <arg name="goal_finder"                              default="True"/>
   <arg name="global_path_planner"                      default="True"/>
   <arg name="planning_manager"                         default="True"/>
+  <arg name="go_to_position_2D_nav_goal"               default="True"/>
   <arg name="social_mpc"                               default="True"/>
   <arg name="social_rl"                                default="False"/>
 
@@ -22,6 +23,7 @@
   <arg name="robot_frame"                              default="base_footprint"/>
   <arg name="global_occupancy_map_topic"               default="global_map"/>
   <arg name="local_occupancy_map_topic"                default="local_static_cost_map"/>
+  <arg name="local_occupancy_map_topic_rl"             default="local_map_rl_navigation"/>
   <arg name="local_social_cost_map_topic"              default="local_social_cost_map"/>
   <arg name="target_id_absent_treshold"                default="3"/>
   
@@ -85,6 +87,7 @@
       <arg name="map_frame"                         value="$(arg map_frame)"/>
       <arg name="robot_frame"                       value="$(arg robot_frame)"/>
       <arg name="local_occupancy_map_topic"         value="$(arg local_occupancy_map_topic)"/>
+      <arg name="local_occupancy_map_topic_rl"      value="$(arg local_occupancy_map_topic_rl)"/>
       <arg name="local_social_cost_map_topic"       value="$(arg local_social_cost_map_topic)"/>
       <arg name="timer_error_max_time"              value="$(arg timer_error_max_time)"/>
       <arg name="intermediate_waypoint_goal_frame"  value="$(arg intermediate_waypoint_goal_frame)"/>
@@ -187,4 +190,8 @@
     </include>
   </group>
 
+  <group if="$(arg go_to_position_2D_nav_goal)">
+    <node pkg="robot_behavior" type="behavior_go_to_position_2D_nav_goal_main.py" name="behavior_go_to_position_2D_nav_goal" output="screen"/>
+  </group>
+
 </launch>

src/robot_behavior/scripts/behavior_go_to_position_2D_nav_goal_main.py

+import roslib
+import rospy
+import tf
+
+from geometry_msgs.msg import PoseStamped
+from spring_msgs.msg import GoToPositionAction,GoToPositionActionGoal
+#
+# This node takes a navigation goal published from rviz (topic /move_base_simple/goal)
+# and send it to our behavior_generator (topic /behavior_generator/go_to_position_action/goal)
+#
+def callback(data):
+    global running
+    global pub
+    newgoal = GoToPositionActionGoal()
+    # we get goal orientation in quaternion form, we need to convert into euler and get the last of the 3 values
+    theta=tf.transformations.euler_from_quaternion([data.pose.orientation.x, data.pose.orientation.y, data.pose.orientation.z, data.pose.orientation.w], axes='sxyz')
+    newgoal.goal.go_to_target_pose = [data.pose.position.x, data.pose.position.y, theta[-1], 0.0, 1.0]
+    newgoal.goal.timeout= 500
+    newgoal.goal.continuous=False
+    pub.publish(newgoal)
+    running=True
+
+
+if __name__ == '__main__':
+    running=False
+    rospy.init_node('rviz_goal_listener', anonymous=True)
+
+    rospy.Subscriber("/move_base_simple/goal", PoseStamped, callback)
+    pub=rospy.Publisher('/behavior_generator/go_to_position_action/goal', GoToPositionActionGoal, queue_size=1, latch=True)
+    rospy.spin()