Merge pull request #4 from Brazilian-Institute-of-Robotics/maze_solver

Maze solver

docker-entrypoint.sh

@@ -0,0 +1,9 @@
+#!/usr/bin/env bash
+
+set -e
+
+find . \! -user doogie -exec chown doogie '{}' +
+
+source "/opt/ros/$ROS_DISTRO/setup.bash"
+
+exec gosu doogie "$@"

dockerfile

@@ -0,0 +1,32 @@
+FROM ros:kinetic-ros-base
+
+ARG DOCKER_USER=doogie
+
+RUN useradd -s /bin/bash ${DOCKER_USER}
+
+# install gosu
+RUN set -eux; \
+    apt-get update; \
+    apt-get install -y gosu; \
+    rm -rf /var/lib/apt/lists/*; \
+    gosu nobody true
+
+# install common dev tools
+RUN export DEBIAN_FRONTEND=noninteractive; \
+    apt-get update && \
+    apt-get install -y --no-install-recommends \
+    bash-completion \
+    pkg-config \
+    git \
+    vim \
+    && rm -rf /var/lib/apt/lists/*
+
+RUN mkdir /home/${DOCKER_USER} && chown ${DOCKER_USER}:${DOCKER_USER} /home/${DOCKER_USER};
+
+WORKDIR /home/${DOCKER_USER}
+
+COPY docker-entrypoint.sh /usr/bin/docker-entrypoint.sh
+
+ENTRYPOINT ["docker-entrypoint.sh"]
+
+CMD ["bash"]

doogie_gazebo/CMakeLists.txt

@@ -4,17 +4,27 @@ project(doogie_gazebo)
 add_compile_options(-std=c++11)
 
 SET(CATKIN_BUILD_DEPS
-  roscpp
-  sensor_msgs
+  angles
   doogie_msgs
+  doogie_localization
+  gazebo_msgs
+  geometry_msgs
   message_filters
+  nav_msgs
+  pluginlib
+  roscpp
+  sensor_msgs
+  tf2
+  tf2_geometry_msgs
+  tf2_ros
 )
 
 find_package(catkin REQUIRED COMPONENTS ${CATKIN_BUILD_DEPS})
 find_package(Boost REQUIRED COMPONENTS system)
 
 catkin_package(
   INCLUDE_DIRS include
+  LIBRARIES gazebo_ground_truth
   CATKIN_DEPENDS ${CATKIN_BUILD_DEPS}
   DEPENDS Boost
 )
@@ -25,6 +35,13 @@ include_directories(
   ${Boost_INCLUDE_DIRS}
 )
 
+add_library(gazebo_ground_truth
+  src/ground_truth.cpp
+)
+target_link_libraries(gazebo_ground_truth
+  ${catkin_LIBRARIES}
+)
+
 add_executable(ir_sensor_data_acc_node
   src/ir_sensor_data_acc_node.cpp  
   src/ir_sensor_data_acc.cpp

doogie_gazebo/ground_truth_plugin_description.xml

@@ -0,0 +1,9 @@
+<library path="lib/libgazebo_ground_truth">
+
+  <class name="doogie_gazebo/GroundTruth" type="doogie_gazebo::GroundTruth" base_class_type="doogie_localization::BaseLocalization">
+    <description>
+      A gazebo ground truth implementation using nav_msgs/Odom msg.
+    </description>
+  </class>
+
+</library>

doogie_gazebo/include/doogie_gazebo/ground_truth.hpp

@@ -0,0 +1,48 @@
+#ifndef DOOGIE_GAZEBO_GROUND_TRUTH_HPP_
+#define DOOGIE_GAZEBO_GROUND_TRUTH_HPP_
+
+#include <gazebo_msgs/ModelStates.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <nav_msgs/Odometry.h>
+
+#include <message_filters/subscriber.h>
+#include <tf2_ros/message_filter.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/transform_listener.h>
+
+#include "doogie_localization/base_localization.hpp"
+
+namespace doogie_gazebo {
+
+class GroundTruth : public doogie_localization::BaseLocalization {
+ public:
+  GroundTruth();
+  explicit GroundTruth(const std::string& odom_frame);
+  virtual ~GroundTruth() = default;
+  void odomCallBack(const nav_msgs::OdometryConstPtr &odometry_msg);
+  double getCurrentXPosition() override;
+  double getCurrentYPosition() override;
+  double getCurrentYawOrientation() override;
+  double getCurrentNormalizedYawOrientation() override;
+
+ private:
+  ros::NodeHandle nh_;
+  ros::NodeHandle pn_{"~"};
+
+  std::string odom_frame_;
+
+  tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_listener_{tf_buffer_};
+  tf2_ros::TransformBroadcaster tf_broadcaster_;  
+
+  message_filters::Subscriber<nav_msgs::Odometry> odom_sub_;
+  tf2_ros::MessageFilter<nav_msgs::Odometry> tf2_filter_;
+
+  geometry_msgs::PoseStamped current_pose_;
+  geometry_msgs::Twist current_velocity_;
+};
+
+}  // doogie_gazebo
+
+#endif // DOOGIE_GAZEBO_GROUND_TRUTH_HPP_

doogie_gazebo/include/doogie_gazebo/ir_sensor_data_acc.hpp

@@ -11,7 +11,7 @@
 #include <message_filters/sync_policies/approximate_time.h>
 
 #include <sensor_msgs/Range.h>
-#include "doogie_msgs/DoogiePosition.h"
+#include "doogie_msgs/DoogiePose.h"
 
 namespace doogie_gazebo {
 
@@ -65,7 +65,7 @@ class IRSensorDataAcc {
    * 
    * @param msg Not used in this application.
    */
-  void UpdateMatrixCallback(const doogie_msgs::DoogiePositionConstPtr& msg);
+  void UpdateMatrixCallback(const doogie_msgs::DoogiePoseConstPtr& msg);
 
   /** Global NodeHandle to publishers and subsribers. */
   ros::NodeHandle nh_;

doogie_gazebo/launch/robot_launch.launch

@@ -7,12 +7,12 @@
   <arg name="gui" default="true"/>
   <arg name="debug" default="false"/>
   <arg name="physics" default="ode"/>
-  <arg name="verbose" default="false"/>
+  <arg name="verbose" default="true"/>
   <arg name="world_name" default="worlds/empty.world"/>
 
   <!-- The arguments you can pass spawn node, for example name:=mouse_1 -->
-  <arg name="robot_name" default="doogie"/>
-  <arg name="robot_param" default="$(arg robot_name)/robot_description"/> 
+  <arg name="namespace" default="doogie"/>
+  <arg name="robot_param" default="$(arg namespace)/robot_description"/> 
 
   <arg name="maze_name" default="minus"/> 
 
@@ -43,36 +43,29 @@
     <arg name="robot_param" value="$(arg robot_param)"/>
   </include>
 
-  <group ns="$(arg robot_name)">
-    <!--Note that because of 'group ns' all doogie params will be prefixed by
+  <group ns="$(arg namespace)">
+    <!--NOTE: all names [such as nodes, topics, params...] called inside '<group ns>' will be prefixed by
     /doogie (e.g., /doogie/robot_description) -->
 
+    <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher"/>
+
     <rosparam file="$(find doogie_description)/config/mazes.yaml" command="load"/>
 
     <node name="spawn_model" pkg="gazebo_ros" type="spawn_model" output="screen"
-      args="-urdf -param /$(arg robot_param) -model $(arg robot_name) 
+      args="-urdf -param /$(arg robot_param) -model $(arg namespace)
       -x $(arg x) -y $(arg y) -z $(arg z) 
       -R $(arg R) -P $(arg P) -Y $(arg Y)"/>
 
+    <node pkg="tf2_ros" type="static_transform_publisher" name="world_to_odom" 
+      args="$(arg x) $(arg y) $(arg z) 
+            $(arg Y) $(arg P) $(arg R) 
+            world odom"/>
+
+    <!-- wheel controller -->
     <include file="$(find doogie_control)/launch/robot_control.launch">
-      <arg name="ns" value="$(arg robot_name)"/>
-    </include>
-
-    <include file="$(find doogie_navigation)/launch/move_base.launch">
-      <arg name="ns" value="$(arg robot_name)"/>
+      <arg name="ns" value="$(arg namespace)"/>
     </include>
 
-    <include file="$(find doogie_perception)/launch/doogie_perception.launch">
-      <arg name="ns" value="$(arg robot_name)"/>
-    </include>
-
-    <node name="ir_sensor_data_acc_node" pkg="doogie_gazebo" type="ir_sensor_data_acc_node" output="screen">
-      <rosparam file="$(find doogie_gazebo)/config/sensor_commons.yaml" command="load"/>
-    </node>
-
-    <include file="$(find doogie_rviz)/launch/maze_map.launch">
-      <arg name="ns" value="$(arg robot_name)"/>
-    </include>
   </group>
 
 </launch>

doogie_gazebo/launch/sim_move_base_interface.launch

@@ -0,0 +1,34 @@
+<?xml version="1.0"?>
+<launch>
+
+  <arg name="namespace" default="doogie"/>
+
+  <include file="$(find doogie_gazebo)/launch/robot_launch.launch">
+    <arg name="namespace" value="$(arg namespace)"/>
+  </include>
+
+  <group ns="$(arg namespace)">
+    <!--Note: all names [ such as nodes, topics, params...] called inside '<group ns>' will be prefixed by
+    /doogie (e.g., /doogie/robot_description) -->
+
+    <!-- doogie_move_in_maze controller -->
+    <include file="$(find doogie_navigation)/launch/move_in_maze.launch"/>
+
+    <!-- doogie_perception -->
+    <include file="$(find doogie_perception)/launch/doogie_perception.launch">
+      <arg name="ns" value="$(arg namespace)"/>
+    </include>
+
+    <!-- wall_distances publisher -->
+    <node name="ir_sensor_data_acc_node" pkg="doogie_gazebo" type="ir_sensor_data_acc_node" output="screen">
+      <rosparam file="$(find doogie_gazebo)/config/sensor_commons.yaml" command="load"/>
+    </node>
+
+    <!-- rviz occupancy grid -->
+    <include file="$(find doogie_rviz)/launch/maze_map.launch">
+      <arg name="ns" value="$(arg namespace)"/>
+    </include>
+
+  </group>
+
+</launch>

doogie_gazebo/launch/sim_solver_application.launch

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<launch>
+
+  <arg name="namespace" default="doogie"/>
+
+  <include file="$(find doogie_gazebo)/launch/sim_move_base_interface.launch">
+    <arg name="namespace" value="$(arg namespace)"/>
+  </include>
+
+  <group ns="$(arg namespace)">
+    <!--Note: all names [ such as nodes, topics, params...] called inside '<group ns>' will be prefixed by
+    /doogie (e.g., /doogie/robot_description) -->
+
+    <include file="$(find doogie_navigation)/launch/maze_solver.launch"/>
+
+  </group>
+
+</launch>

doogie_gazebo/package.xml

@@ -14,15 +14,29 @@
 
   <buildtool_depend>catkin</buildtool_depend>
 
-  <depend>roscpp</depend>
-  <depend>sensor_msgs</depend>
+  <depend>angles</depend>
+  <depend>doogie_localization</depend>
   <depend>doogie_msgs</depend>
+  <depend>gazebo_msgs</depend>
+  <depend>geometry_msgs</depend>
   <depend>message_filters</depend>
-
+  <depend>nav_msgs</depend>
+  <depend>pluginlib</depend>
+  <depend>roscpp</depend>
+  <depend>sensor_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_geometry_msgs</depend>
+
+  <exec_depend>doogie_control</exec_depend>
+  <exec_depend>doogie_description</exec_depend>
+  <exec_depend>doogie_navigation</exec_depend>
+  <exec_depend>gazebo_plugins</exec_depend>
   <exec_depend>gazebo_ros</exec_depend>
   <exec_depend>gazebo_ros_control</exec_depend>
-  <exec_depend>gazebo_plugins</exec_depend>
-  <exec_depend>doogie_description</exec_depend>
-  <exec_depend>doogie_control</exec_depend>
+
+  <export>
+    <doogie_localization plugin="${prefix}/ground_truth_plugin_description.xml"/>
+  </export>
 
 </package>

doogie_gazebo/src/ground_truth.cpp

@@ -0,0 +1,58 @@
+#include "doogie_gazebo/ground_truth.hpp"
+
+#include <angles/angles.h>
+#include <pluginlib/class_list_macros.h>
+#include <tf2/utils.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+
+namespace doogie_gazebo {
+
+GroundTruth::GroundTruth() : GroundTruth("odom") {}
+
+GroundTruth::GroundTruth(const std::string& odom_frame)
+: odom_frame_(odom_frame)
+, tf2_filter_(odom_sub_, tf_buffer_, odom_frame, 1, nullptr) {
+  std::string topic_name = "/gazebo/ground_truth";
+  pn_.getParam("input_topic", topic_name);
+  odom_sub_.subscribe(nh_, topic_name, 1);
+  tf2_filter_.registerCallback(boost::bind(&GroundTruth::odomCallBack, this, _1));
+}
+
+void GroundTruth::odomCallBack(const nav_msgs::OdometryConstPtr &odometry_msg) {
+  ROS_DEBUG_THROTTLE(0.5, "odom callback");
+  current_pose_.header = odometry_msg->header;
+  current_pose_.pose = odometry_msg->pose.pose;
+  current_velocity_ = odometry_msg->twist.twist;
+
+  tf_buffer_.transform(current_pose_, current_pose_, odom_frame_);
+  ROS_DEBUG_THROTTLE(1.0, "Doogie Position(x:%f y:%f z:%f)\n", 
+            current_pose_.pose.position.x,
+            current_pose_.pose.position.y,
+            current_pose_.pose.position.z);
+
+  tf2::Stamped<tf2::Transform> temp;
+  tf2::fromMsg(current_pose_, temp);
+  geometry_msgs::TransformStamped gt_tf = tf2::toMsg(temp);
+  gt_tf.child_frame_id = "ground_truth";
+  tf_broadcaster_.sendTransform(gt_tf);
+}
+
+double GroundTruth::getCurrentXPosition() {
+  return current_pose_.pose.position.x;
+}
+
+double GroundTruth::getCurrentYPosition() {
+  return current_pose_.pose.position.y;
+}
+
+double GroundTruth::getCurrentYawOrientation() {
+  return tf2::getYaw(current_pose_.pose.orientation);
+}
+
+double GroundTruth::getCurrentNormalizedYawOrientation() {
+  return angles::normalize_angle(getCurrentYawOrientation());
+}
+
+}  // doogie_gazebo
+
+PLUGINLIB_EXPORT_CLASS(doogie_gazebo::GroundTruth, doogie_localization::BaseLocalization)