RoboBuggy

[x] 1. rename buggymsgs into buggycommon

[ ] 2. add buggymaps package

[ ] 3. Use parameter server to load maps

     - mapname/map?

     - offset

[ ] 4. rearchitect simulator into "sim" and "visualizer"

[ ] 5. Document each package, fill out package.xml and cmakelist

   - Include .h files in all cmakelists; buggysim

[ ] 6. be able to save run data into a new map.

[ ] 7. design map-merge facilities

cmake_minimum_required(VERSION 2.8.3)

project(buggymaps)

## Find catkin macros and libraries

## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)

## is used, also find other catkin packages

find_package(catkin REQUIRED COMPONENTS

  roscpp

  rospy

  std_msgs

  buggycommon

)

find_package(Qt4 4.6 REQUIRED COMPONENTS QtCore QtGui)

include(${QT_USE_FILE})

find_package(Boost REQUIRED COMPONENTS thread)

include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${QT_INCLUDE_DIRS})

link_directories(${catkin_LIBRARY_DIRS})

# Load components

set(buggymaps_HDRS

  src/buggymaps.h

)

set(buggymaps_SRCS

  src/buggymaps.cpp

)

# load QT components

qt4_wrap_cpp(buggymaps_MOCS ${buggymaps_HDRS})

catkin_package(

  LIBRARIES buggymaps

)

###########

## Build ##

###########

## Specify additional locations of header files

## Your package locations should be listed before other locations

# include_directories(include)

#include_directories(

#  ${catkin_INCLUDE_DIRS}

#)

## Declare a cpp library

add_library(buggymaps

  ${buggymaps_SRCS}

  ${buggymaps_MOCS}

)

target_link_libraries(buggymaps

  ${catkin_LIBRARIES}

  ${boost_LIBRARIES}

  ${QT_LIBRARIES}

)

## Declare a cpp executable

# add_executable(buggymaps_node src/buggymaps_node.cpp)

## Add cmake target dependencies of the executable/library

## as an example, message headers may need to be generated before nodes

# add_dependencies(buggymaps_node buggymaps_generate_messages_cpp)

## Specify libraries to link a library or executable target against

# target_link_libraries(buggymaps_node

#   ${catkin_LIBRARIES}

# )

#############

## Install ##

#############

# all install targets should use catkin DESTINATION variables

# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html

## Mark executable scripts (Python etc.) for installation

## in contrast to setup.py, you can choose the destination

# install(PROGRAMS

#   scripts/my_python_script

#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}

# )

## Mark executables and/or libraries for installation

# install(TARGETS buggymaps buggymaps_node

#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}

#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}

#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}

# )

## Mark cpp header files for installation

# install(DIRECTORY include/${PROJECT_NAME}/

#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}

#   FILES_MATCHING PATTERN "*.h"

#   PATTERN ".svn" EXCLUDE

# )

## Mark other files for installation (e.g. launch and bag files, etc.)

# install(FILES

#   # myfile1

#   # myfile2

#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}

# )

#############

## Testing ##

#############

## Add gtest based cpp test target and link libraries

# catkin_add_gtest(${PROJECT_NAME}-test test/test_buggymaps.cpp)

# if(TARGET ${PROJECT_NAME}-test)

#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})

# endif()

## Add folders to be run by python nosetests

# catkin_add_nosetests(test)

<?xml version="1.0" encoding="UTF-8"?>

<!DOCTYPE QtCreatorProject>

<!-- Written by Qt Creator 2.4.1, 2014-01-01T19:34:35. -->

<qtcreator>

 <data>

  <variable>ProjectExplorer.Project.ActiveTarget</variable>

  <value type="int">0</value>

 </data>

 <data>

  <variable>ProjectExplorer.Project.EditorSettings</variable>

  <valuemap type="QVariantMap">

   <value type="bool" key="EditorConfiguration.AutoIndent">true</value>

   <value type="bool" key="EditorConfiguration.AutoSpacesForTabs">false</value>

   <valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.0">

    <value type="QString" key="language">Cpp</value>

    <valuemap type="QVariantMap" key="value">

     <value type="QString" key="CurrentPreferences">CppGlobal</value>

    </valuemap>

   </valuemap>

   <valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.1">

    <value type="QString" key="language">QmlJS</value>

    <valuemap type="QVariantMap" key="value">

     <value type="QString" key="CurrentPreferences">QmlJSGlobal</value>

    </valuemap>

   </valuemap>

   <value type="int" key="EditorConfiguration.CodeStyle.Count">2</value>

   <value type="QByteArray" key="EditorConfiguration.Codec">System</value>

   <value type="bool" key="EditorConfiguration.ConstrainTooltips">false</value>

   <value type="int" key="EditorConfiguration.IndentSize">4</value>

   <value type="bool" key="EditorConfiguration.MouseNavigation">true</value>

   <value type="int" key="EditorConfiguration.PaddingMode">1</value>

   <value type="bool" key="EditorConfiguration.ScrollWheelZooming">true</value>

   <value type="int" key="EditorConfiguration.SmartBackspaceBehavior">2</value>

   <value type="bool" key="EditorConfiguration.SpacesForTabs">true</value>

   <value type="int" key="EditorConfiguration.TabKeyBehavior">0</value>

   <value type="int" key="EditorConfiguration.TabSize">8</value>

   <value type="bool" key="EditorConfiguration.UseGlobal">true</value>

   <value type="int" key="EditorConfiguration.Utf8BomBehavior">1</value>

   <value type="bool" key="EditorConfiguration.addFinalNewLine">true</value>

   <value type="bool" key="EditorConfiguration.cleanIndentation">true</value>

   <value type="bool" key="EditorConfiguration.cleanWhitespace">true</value>

   <value type="bool" key="EditorConfiguration.inEntireDocument">false</value>

  </valuemap>

 </data>

 <data>

  <variable>ProjectExplorer.Project.PluginSettings</variable>

  <valuemap type="QVariantMap"/>

 </data>

 <data>

  <variable>ProjectExplorer.Project.Target.0</variable>

  <valuemap type="QVariantMap">

   <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Desktop</value>

   <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>

   <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">CMakeProjectManager.DefaultCMakeTarget</value>

   <value type="int" key="ProjectExplorer.Target.ActiveBuildConfiguration">0</value>

   <value type="int" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>

   <value type="int" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>

   <valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.0">

    <value type="QString" key="CMakeProjectManager.CMakeBuildConfiguration.BuildDirectory">/home/tahm/buggyworkspace/build/buggymaps</value>

    <value type="QString" key="CMakeProjectManager.CMakeBuildConfiguration.ToolChain">ProjectExplorer.ToolChain.Gcc:/usr/bin/g++.x86-linux-generic-elf-32bit./usr/bin/gdb</value>

    <value type="QString" key="ProjectExplorer.BuildCOnfiguration.ToolChain">ProjectExplorer.ToolChain.Gcc:/usr/bin/g++.x86-linux-generic-elf-32bit./usr/bin/gdb</value>

    <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">

     <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">

      <value type="QString" key="CMakeProjectManager.MakeStep.AdditionalArguments"></value>

      <valuelist type="QVariantList" key="CMakeProjectManager.MakeStep.BuildTargets"/>

      <value type="bool" key="CMakeProjectManager.MakeStep.Clean">false</value>

      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>

      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>

      <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">CMakeProjectManager.MakeStep</value>

     </valuemap>

     <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">1</value>

     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Build</value>

     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>

     <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>

    </valuemap>

    <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">

     <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">

      <value type="QString" key="CMakeProjectManager.MakeStep.AdditionalArguments">clean</value>

      <valuelist type="QVariantList" key="CMakeProjectManager.MakeStep.BuildTargets"/>

      <value type="bool" key="CMakeProjectManager.MakeStep.Clean">true</value>

      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>

      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>

      <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">CMakeProjectManager.MakeStep</value>

     </valuemap>

     <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">1</value>

     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Clean</value>

     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>

     <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>

    </valuemap>

    <value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>

    <value type="bool" key="ProjectExplorer.BuildConfiguration.ClearSystemEnvironment">false</value>

    <valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>

    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">all</value>

    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>

    <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">CMakeProjectManager.CMakeBuildConfiguration</value>

   </valuemap>

   <value type="int" key="ProjectExplorer.Target.BuildConfigurationCount">1</value>

   <valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">

    <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">

     <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">0</value>

     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy</value>

     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>

     <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>

    </valuemap>

    <value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>

    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">No deployment</value>

    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>

    <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.DefaultDeployConfiguration</value>

   </valuemap>

   <value type="int" key="ProjectExplorer.Target.DeployConfigurationCount">1</value>

   <valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">

    <value type="bool" key="Analyzer.Project.UseGlobal">true</value>

    <value type="bool" key="Analyzer.Project.UseGlobal">true</value>

    <valuelist type="QVariantList" key="Analyzer.Valgrind.AddedSuppressionFiles"/>

    <valuelist type="QVariantList" key="Analyzer.Valgrind.AddedSuppressionFiles"/>

    <value type="bool" key="Analyzer.Valgrind.Callgrind.CollectBusEvents">false</value>

    <value type="bool" key="Analyzer.Valgrind.Callgrind.CollectBusEvents">false</value>

    <value type="bool" key="Analyzer.Valgrind.Callgrind.CollectSystime">false</value>

    <value type="bool" key="Analyzer.Valgrind.Callgrind.CollectSystime">false</value>

    <value type="bool" key="Analyzer.Valgrind.Callgrind.EnableBranchSim">false</value>

    <value type="bool" key="Analyzer.Valgrind.Callgrind.EnableBranchSim">false</value>

    <value type="bool" key="Analyzer.Valgrind.Callgrind.EnableCacheSim">false</value>

    <value type="bool" key="Analyzer.Valgrind.Callgrind.EnableCacheSim">false</value>

    <value type="bool" key="Analyzer.Valgrind.Callgrind.EnableEventToolTips">true</value>

    <value type="bool" key="Analyzer.Valgrind.Callgrind.EnableEventToolTips">true</value>

    <value type="double" key="Analyzer.Valgrind.Callgrind.MinimumCostRatio">0.01</value>

    <value type="double" key="Analyzer.Valgrind.Callgrind.MinimumCostRatio">0.01</value>

    <value type="double" key="Analyzer.Valgrind.Callgrind.VisualisationMinimumCostRatio">10</value>

    <value type="double" key="Analyzer.Valgrind.Callgrind.VisualisationMinimumCostRatio">10</value>

    <value type="bool" key="Analyzer.Valgrind.FilterExternalIssues">true</value>

    <value type="bool" key="Analyzer.Valgrind.FilterExternalIssues">true</value>

    <value type="int" key="Analyzer.Valgrind.NumCallers">25</value>

    <value type="int" key="Analyzer.Valgrind.NumCallers">25</value>

    <valuelist type="QVariantList" key="Analyzer.Valgrind.RemovedSuppressionFiles"/>

    <valuelist type="QVariantList" key="Analyzer.Valgrind.RemovedSuppressionFiles"/>

    <value type="bool" key="Analyzer.Valgrind.TrackOrigins">true</value>

    <value type="bool" key="Analyzer.Valgrind.TrackOrigins">true</value>

    <value type="QString" key="Analyzer.Valgrind.ValgrindExecutable">valgrind</value>

    <value type="QString" key="Analyzer.Valgrind.ValgrindExecutable">valgrind</value>

    <valuelist type="QVariantList" key="Analyzer.Valgrind.VisibleErrorKinds">

     <value type="int">0</value>

     <value type="int">1</value>

     <value type="int">2</value>

     <value type="int">3</value>

     <value type="int">4</value>

     <value type="int">5</value>

     <value type="int">6</value>

     <value type="int">7</value>

     <value type="int">8</value>

     <value type="int">9</value>

     <value type="int">10</value>

     <value type="int">11</value>

     <value type="int">12</value>

     <value type="int">13</value>

     <value type="int">14</value>

    </valuelist>

    <valuelist type="QVariantList" key="Analyzer.Valgrind.VisibleErrorKinds">

     <value type="int">0</value>

     <value type="int">1</value>

     <value type="int">2</value>

     <value type="int">3</value>

     <value type="int">4</value>

     <value type="int">5</value>

     <value type="int">6</value>

     <value type="int">7</value>

     <value type="int">8</value>

     <value type="int">9</value>

     <value type="int">10</value>

     <value type="int">11</value>

     <value type="int">12</value>

     <value type="int">13</value>

     <value type="int">14</value>

    </valuelist>

    <value type="QString" key="ProjectExplorer.CustomExecutableRunConfiguration.Arguments"></value>

    <value type="int" key="ProjectExplorer.CustomExecutableRunConfiguration.BaseEnvironmentBase">2</value>

    <value type="QString" key="ProjectExplorer.CustomExecutableRunConfiguration.Executable"></value>

    <value type="bool" key="ProjectExplorer.CustomExecutableRunConfiguration.UseTerminal">false</value>

    <valuelist type="QVariantList" key="ProjectExplorer.CustomExecutableRunConfiguration.UserEnvironmentChanges"/>

    <value type="QString" key="ProjectExplorer.CustomExecutableRunConfiguration.WorkingDirectory">%{buildDir}</value>

    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Custom Executable</value>

    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>

    <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>

    <value type="uint" key="RunConfiguration.QmlDebugServerPort">3768</value>

    <value type="bool" key="RunConfiguration.UseCppDebugger">true</value>

    <value type="bool" key="RunConfiguration.UseQmlDebugger">false</value>

    <value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">false</value>

   </valuemap>

   <value type="int" key="ProjectExplorer.Target.RunConfigurationCount">1</value>

  </valuemap>

 </data>

 <data>

  <variable>ProjectExplorer.Project.TargetCount</variable>

  <value type="int">1</value>

 </data>

 <data>

  <variable>ProjectExplorer.Project.Updater.EnvironmentId</variable>

  <value type="QString">{0509d71f-18a9-4f6f-aa06-6baf391d5a04}</value>

 </data>

 <data>

  <variable>ProjectExplorer.Project.Updater.FileVersion</variable>

  <value type="int">10</value>

 </data>

</qtcreator>

<?xml version="1.0"?>

<package>

  <name>buggymaps</name>

  <version>0.0.1</version>

  <description>The buggymaps package</description>

  <!-- One maintainer tag required, multiple allowed, one person per tag --> 

  <!-- Example:  -->

  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->

  <maintainer email="tahm@todo.todo">tahm</maintainer>

  <!-- One license tag required, multiple allowed, one license per tag -->

  <!-- Commonly used license strings: -->

  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->

  <license>TODO</license>

  <!-- Url tags are optional, but mutiple are allowed, one per tag -->

  <!-- Optional attribute type can be: website, bugtracker, or repository -->

  <!-- Example: -->

  <!-- <url type="website">http://wiki.ros.org/buggymaps</url> -->

  <!-- Author tags are optional, mutiple are allowed, one per tag -->

  <!-- Authors do not have to be maintianers, but could be -->

  <!-- Example: -->

  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->

  <!-- The *_depend tags are used to specify dependencies -->

  <!-- Dependencies can be catkin packages or system dependencies -->

  <!-- Examples: -->

  <!-- Use build_depend for packages you need at compile time: -->

  <!--   <build_depend>message_generation</build_depend> -->

  <!-- Use buildtool_depend for build tool packages: -->

  <!--   <buildtool_depend>catkin</buildtool_depend> -->

  <!-- Use run_depend for packages you need at runtime: -->

  <!--   <run_depend>message_runtime</run_depend> -->

  <!-- Use test_depend for packages you need only for testing: -->

  <!--   <test_depend>gtest</test_depend> -->

  <buildtool_depend>catkin</buildtool_depend>

  <build_depend>roscpp</build_depend>

  <build_depend>buggycommon/</build_depend>

  <run_depend>roscpp</run_depend>

  <run_depend>buggycommon/</run_depend>

  <!-- The export tag contains other, unspecified, tags -->

  <export>

    <!-- You can specify that this package is a metapackage here: -->

    <!-- <metapackage/> -->

    <!-- Other tools can request additional information be placed here -->

  </export>

</package>

#include "buggymaps.h"

#include <ros/ros.h>

#include <fstream>

#include <cstdlib>

#include <ctime>

#define DEFAULT_BG_R 0x45

#define DEFAULT_BG_G 0x56

#define DEFAULT_BG_B 0xff

// TODO: add meter_ parameter

namespace buggymaps

{

  bool file_exist(QString fileName)

  {

    std::ifstream infile(fileName.toUtf8().constData());

    return infile.good();

  }

  Map::Map(const std::string map_path,

                   const float x_offset,

                   const float y_offset)

   {

      x_offset_ = x_offset;

      y_offset_ = y_offset;

      // Load Images

      base_path_ = QString(map_path.c_str());

      // Strip all double-slashes

      map_image_path_ = QString((map_path+".bmp").c_str());

      height_image_path_ = QString((map_path+"_height.bmp").c_str());

      // Check if images exist

      if(!file_exist(map_image_path_)) {

          ROS_INFO("Incorrect map path passed in");

          ROS_INFO("Should be of the form:");

          ROS_INFO("/path/to/map/map for map image of name:");

          ROS_INFO("   /path/to/map/map.bmp");

          assert(false);

      }

      if(!file_exist(height_image_path_)) {

          ROS_INFO("Incorrect map path passed in");

          ROS_INFO("Should be of the form:");

          ROS_INFO("/path/to/map/map for map image of name:");

          ROS_INFO("   /path/to/map/map_.bmp");

          assert(false);

      }

      // Find the correct dimensions

      // Check that the image format is correct

      map_image_ = new QImage(map_image_path_);

      height_image_ = new QImage(height_image_path_);

      // Check that images are in bounds

      int map_image_height = map_image_->height();

      int map_image_width = map_image_->width();

      int height_image_height = height_image_->height();

      int height_image_width = height_image_->width();

      if(height_image_height != map_image_height) {

          ROS_INFO("Map image height != height image height");

          assert(false);

      }

      if(height_image_width != map_image_width) {

          ROS_INFO("Map image width != height image width");

          assert(false);

      }

      map_pixel_height_ = map_image_height;

      map_pixel_width_= map_image_width;

      ROS_INFO("Loading map with path '%s'",base_path_.toUtf8().constData());

   }

   Map::~Map()

   {

       delete map_image_;

       delete height_image_;

   }

   int Map::getMapPixelHeight()

   {

      return map_pixel_height_;

   }

   int Map::getMapPixelWidth()

   {

       return map_pixel_width_;

   }

   float Map::getTerrainHeight(float x, float y) {

       ROS_INFO("Unimplemented");

       assert(false);

   }

   float Map::getColor(float x, float y) {

       ROS_INFO("Unimplemented");

       assert(false);

   }

   const QImage Map::getMapImage() {

       return *map_image_;

   }

   const QImage Map::getHeightImage() {

       return *height_image_;

   }

   float Map::getXOffset() {

       return x_offset_;

   }

   float Map::getYOffset() {

       return y_offset_;

   }

}

#ifndef BUGGYMAPS_BUGGYMAPS_H

#define BUGGYMAPS_BUGGYMAPS_H

#include <QImage>

namespace buggymaps

{

class Map

{

 public:

  Map(const std::string map_path,

      const float x_offset,

      const float y_offset);

  ~Map();

  int getMapPixelHeight();

  int getMapPixelWidth();

  float getTerrainHeight(float x, float y);

  float getColor(float x, float y);

  const QImage getMapImage();

  const QImage getHeightImage();

  float getXOffset();

  float getYOffset();

 private:

  QImage* map_image_;

  QImage* height_image_;

  QString base_path_;

  QString map_image_path_;

  QString height_image_path_;

  int map_pixel_height_;

  int map_pixel_width_;

  float x_offset_;

  float y_offset_;

};

}

#endif