diff --git a/CMakeLists.txt b/CMakeLists.txt
index cfb3fc6d4a1e7cb709636903af2e74cd73b40b4e..19eaaa88a898a69188596dedd6b829031787f87b 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,6 +1,8 @@
cmake_minimum_required(VERSION 2.8.3)
project(iri_akp_tools_companion)
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
+
## Find catkin macros and libraries
find_package(catkin REQUIRED)
# ********************************************************************
@@ -12,6 +14,12 @@ find_package(catkin REQUIRED COMPONENTS
nav_msgs
sensor_msgs
tf
+ iri_perception_msgs
+ pluginlib
+ nav_core
+ costmap_2d
+ visualization_msgs
+ std_msgs
)
## System dependencies are found with CMake's conventions
@@ -21,6 +29,18 @@ find_package(catkin REQUIRED COMPONENTS
# Add system and labrobotica dependencies here
# ********************************************************************
# find_package(<dependency> REQUIRED)
+find_package(cmake_modules REQUIRED)
+find_package(Eigen QUIET)
+if(Eigen_FOUND)
+ MESSAGE("Eigen found in CMake modules:" ${Eigen_INCLUDE_DIRS})
+ELSE(Eigen_FOUND)
+ #INCLUDE_DIRECTORIES(/usr/include/eigen3)
+ #INCLUDE_DIRECTORIES(/usr/local/include/eigen3)
+ MESSAGE("Eigen not found, adding manually directories. BE CAREFULL to have installed eigen there")
+ SET(Eigen_INCLUDE_DIRS /usr/include/eigen3 /usr/local/include/eigen3)
+ENDIF(Eigen_FOUND)
+INCLUDE_DIRECTORIES(${Eigen_INCLUDE_DIRS})
+
# ********************************************************************
# Add topic, service and action definition here
@@ -56,12 +76,13 @@ find_package(catkin REQUIRED COMPONENTS
# Add the dynamic reconfigure file
# ********************************************************************
#generate_dynamic_reconfigure_options(cfg/IriAkpTools.cfg)
-generate_dynamic_reconfigure_options(cfg/DiffPlatformSimulator.cfg cfg/FakeLaserGen.cfg)
-
+generate_dynamic_reconfigure_options(cfg/DiffPlatformSimulator.cfg cfg/FakeLaserGen.cfg cfg/FakeAkpLocalPlanner.cfg)
+#cfg/FakeAkpLocalPlanner.cfg
# ********************************************************************
# Add run time dependencies here
# ********************************************************************
catkin_package(
+ LIBRARIES iri_akp_local_planner
# INCLUDE_DIRS
# LIBRARIES
# ********************************************************************
@@ -73,10 +94,16 @@ catkin_package(
nav_msgs
sensor_msgs
tf
+# iri_perception_msgs
+# pluginlib
+# nav_core
+# costmap_2d
+# visualization_msgs
+# std_msgs
# ********************************************************************
# Add system and labrobotica run time dependencies here
# ********************************************************************
-# DEPENDS
+# DEPENDS
)
###########
@@ -88,6 +115,7 @@ catkin_package(
# ********************************************************************
include_directories(include)
include_directories(${catkin_INCLUDE_DIRS})
+#include_directories(${people_prediction_INCLUDE_DIR} ${EIGEN_INCLUDE_DIR})
# include_directories(${<dependency>_INCLUDE_DIR})
## Declare a cpp library
@@ -96,13 +124,16 @@ include_directories(${catkin_INCLUDE_DIRS})
## Declare a cpp executable
add_executable(diff_platform_simulator src/diff_platform_simulator_alg.cpp src/diff_platform_simulator_alg_node.cpp)
add_executable(fake_laser_gen src/fake_laser_gen_alg.cpp src/fake_laser_gen_alg_node.cpp)
+#add_executable(fake_akp_local_planner_alg src/fake_akp_local_planner_alg.cpp src/fake_akp_local_planner_alg_node.cpp)
# ********************************************************************
# Add the libraries
# ********************************************************************
target_link_libraries(diff_platform_simulator ${catkin_LIBRARIES})
target_link_libraries(fake_laser_gen ${catkin_LIBRARIES})
+#target_link_libraries(fake_akp_local_planner ${catkin_LIBRARIES})
# target_link_libraries(${PROJECT_NAME} ${<dependency>_LIBRARY})
+#target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${people_prediction_LIBRARY})
# ********************************************************************
# Add message headers dependencies
diff --git a/launch/diff_platform_simulator.launch b/launch/diff_platform_simulator.launch
index 3fc3b2922dd62e33f28090a6b9a06743ccaddeac..4643c6037d7244898f87f4ecf343177277dd851e 100644
--- a/launch/diff_platform_simulator.launch
+++ b/launch/diff_platform_simulator.launch
@@ -3,7 +3,7 @@
<node pkg ="iri_akp_tools_companion"
type="diff_platform_simulator"
- name="platform_simulator"
+ name="$(optenv ROBOT tibi)_platform_simulator"
output="screen">
<param name="robot" type="string" value="$(optenv ROBOT tibi)" />
<remap from="~cmd_vel"
diff --git a/launch/diff_platform_simulator2.launch b/launch/diff_platform_simulator2.launch
new file mode 100644
index 0000000000000000000000000000000000000000..6e5c579520e550858c4334308330caf13356d6f1
--- /dev/null
+++ b/launch/diff_platform_simulator2.launch
@@ -0,0 +1,15 @@
+<!-- -->
+<launch>
+
+ <node pkg ="iri_akp_tools_companion"
+ type="diff_platform_simulator"
+ name="$(optenv ROBOT dabo)_platform_simulator"
+ output="screen">
+ <param name="robot" type="string" value="$(optenv ROBOT dabo)" />
+ <remap from="~cmd_vel"
+ to="/$(optenv ROBOT dabo)/segway/cmd_vel" />
+ <remap from="~odom"
+ to="/$(optenv ROBOT dabo)/segway/odom" />
+ </node>
+
+</launch>
diff --git a/launch/fake_laser_gen.launch b/launch/fake_laser_gen.launch
index e29d506126f69fc507d491d46f60e83f0da5ef95..3ccc1fd2b0944ccc758ca19705d941976d31f91a 100644
--- a/launch/fake_laser_gen.launch
+++ b/launch/fake_laser_gen.launch
@@ -3,7 +3,7 @@
<node pkg ="iri_akp_tools_companion"
type="fake_laser_gen"
- name="fake_laser_gen"
+ name="$(optenv ROBOT tibi)_fake_laser_gen"
output="screen">
<param name="laser_mode" value="0" />
<remap from="~scan" to="/$(optenv ROBOT tibi)/sensors/front_laser_scan"/>
diff --git a/launch/fake_laser_gen2.launch b/launch/fake_laser_gen2.launch
new file mode 100644
index 0000000000000000000000000000000000000000..e03124e5fb9b3c6e03b8adc6261c7eaea72e06a0
--- /dev/null
+++ b/launch/fake_laser_gen2.launch
@@ -0,0 +1,12 @@
+<!-- -->
+<launch>
+
+ <node pkg ="iri_akp_tools_companion"
+ type="fake_laser_gen"
+ name="$(optenv ROBOT dabo)_fake_laser_gen"
+ output="screen">
+ <param name="laser_mode" value="0" />
+ <remap from="~scan" to="/$(optenv ROBOT dabo)/sensors/front_laser_scan"/>
+ </node>
+
+</launch>
diff --git a/launch/fake_localization.launch b/launch/fake_localization.launch
index d6eac8ebb8bc38fc326d89b3e25fde1086057b41..cdfc05635aaf627d6af87531bfc94bc70904ad07 100644
--- a/launch/fake_localization.launch
+++ b/launch/fake_localization.launch
@@ -12,8 +12,8 @@
<param name="base_frame_id" value="/$(optenv ROBOT tibi)/base_footprint" />
<!-- parameters to change the robot initial position, if want the robot in -1 and -8=> delta_x=1.0 ; delta_y=8.0 -->
<param name="delta_x" value="1.0" /> <!-- Normal position (estrechez mitad camino) = delta_x=1.0 ; delta_y=2.0 ; delta_yaw=0.0 -->
- <param name="delta_y" value="2.0" /> <!-- GIRO ESQUINA position (estrechez mitad camino)= delta_x=1.0 ; delta_y=-18.0 ; delta_yaw=1.57 -->
- <param name="delta_yaw" value="0.0" />
+ <param name="delta_y" value="-18.0" /> <!-- GIRO ESQUINA position (estrechez mitad camino)= delta_x=1.0 ; delta_y=-18.0 ; delta_yaw=1.57 -->
+ <param name="delta_yaw" value="1.57" />
<remap from="/odom" to="/$(optenv ROBOT tibi)/segway/odom"/>
<remap from="base_pose_ground_truth" to="/$(optenv ROBOT tibi)/segway/odom"/>
</node>
diff --git a/launch/map_server_local2.launch b/launch/map_server_local2.launch
new file mode 100644
index 0000000000000000000000000000000000000000..46c65b828132d237c331581e226533b4c04c711b
--- /dev/null
+++ b/launch/map_server_local2.launch
@@ -0,0 +1,22 @@
+<!-- -->
+<launch>
+
+ <arg name="map" default="fme_open"/>
+
+ <!-- Map Server -->
+ <!-- published topics: /$(optenv ROBOT tibi)/map -->
+ <!-- subscribed topics: -->
+ <!-- service clients: -->
+ <!-- service servers: -->
+ <!-- action clients: -->
+ <!-- action servers: -->
+ <node name="$(optenv ROBOT dabo)_map_server"
+ pkg ="map_server"
+ type="map_server"
+ args="$(find iri_akp_local_planner_companion)/maps/$(arg map).yaml">
+ <param name="frame_id" value="/map" />
+ <remap from="/map"
+ to="$(optenv ROBOT dabo)/map"/>
+ </node>
+
+</launch>
diff --git a/package.xml b/package.xml
index 65fba47bf82e441ab3572539ed40a463bbb9d54c..0f479b7030e1a2e31c28957165152c6f63a2578e 100644
--- a/package.xml
+++ b/package.xml
@@ -44,14 +44,29 @@
<build_depend>geometry_msgs</build_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
- <build_depend>tf</build_depend>
-
+ <build_depend>tf</build_depend>
+ <!-- <build_depend>iri_perception_msgs</build_depend>
+ <build_depend>pluginlib</build_depend>
+ <build_depend>nav_core</build_depend>
+ <build_depend>costmap_2d</build_depend>
+ <build_depend>visualization_msgs</build_depend>
+ <build_depend>std_msgs</build_depend>
+ <build_depend>cmake_modules</build_depend> -->
+
<run_depend>iri_base_algorithm</run_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>nav_msgs</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>tf</run_depend>
+ <!-- <run_depend>iri_perception_msgs</run_depend>
+ <run_depend>pluginlib</run_depend>
+ <run_depend>nav_core</run_depend>
+ <run_depend>costmap_2d</run_depend>
+ <run_depend>visualization_msgs</run_depend>
+ <run_depend>std_msgs</run_depend>
+ <run_depend>cmake_modules</run_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
diff --git a/src/fake_laser_gen_alg_node.cpp b/src/fake_laser_gen_alg_node.cpp
index fa278df400b0606d24031e7e317e52f8513fe39d..c21a260b1c8d7a6ad601035d5be28d4e6fa5d6b2 100644
--- a/src/fake_laser_gen_alg_node.cpp
+++ b/src/fake_laser_gen_alg_node.cpp
@@ -88,9 +88,9 @@ void FakeLaserGenAlgNode::node_config_update(Config &config, uint32_t level)
LaserScan_msg_.ranges.clear();
LaserScan_msg_.ranges.resize(1436,59.9);
//laser_mode_=FakeLaserGenAlgNode::n_obstacles5; //caso estrechez en medio del camino. Muy estrecho!
- laser_mode_=FakeLaserGenAlgNode::n_obstacles3; //caso estrechez en medio del camino!
+ // laser_mode_=FakeLaserGenAlgNode::n_obstacles3; //caso estrechez en medio del camino!
//laser_mode_=FakeLaserGenAlgNode::n_obstacles6; //caso estrechez en medio del camino!
- //laser_mode_=FakeLaserGenAlgNode::scene_3; // caso pasillo, esquina. (Ancho)
+ laser_mode_=FakeLaserGenAlgNode::scene_3; // caso pasillo, esquina. (Ancho)
//laser_mode_=FakeLaserGenAlgNode::scene_2; // Estrecho! caso pasillo, esquina.
//laser_mode_=FakeLaserGenAlgNode::scene_4; // pasillo esquina, medio ancho, medio estrecho!
@@ -303,19 +303,19 @@ void FakeLaserGenAlgNode::node_config_update(Config &config, uint32_t level)
LaserScan_msg_.ranges[340]= 7.0;
//LaserScan_msg_.ranges[340+40]= 7.0;
// LaserScan_msg_.ranges[660]= 13.25;
- // LaserScan_msg_.ranges[660+20]= 13.0;
- // LaserScan_msg_.ranges[760]= 13.0;
- // LaserScan_msg_.ranges[760+20]= 13.25;
- LaserScan_msg_.ranges[680-5]= 13.15;
- LaserScan_msg_.ranges[680]= 13.15;
- LaserScan_msg_.ranges[680+5]= 13.0;
- LaserScan_msg_.ranges[730]= 13.0;
- LaserScan_msg_.ranges[730+5]= 13.15;
+ LaserScan_msg_.ranges[680-30]= 13.0;
+ LaserScan_msg_.ranges[680-25]= 13.25;
+ LaserScan_msg_.ranges[680-20]= 13.25;
+ LaserScan_msg_.ranges[680-15]= 13.15;
+ LaserScan_msg_.ranges[680-10]= 13.15;
+ LaserScan_msg_.ranges[680-5]= 13.0;
+ LaserScan_msg_.ranges[730+5]= 13.0;
LaserScan_msg_.ranges[730+10]= 13.15;
- // LaserScan_msg_.ranges[1060]= 7.0;
- LaserScan_msg_.ranges[1060+40]= 7.0;
- LaserScan_msg_.ranges[1300]= 7.0;
- LaserScan_msg_.ranges[1300+40]= 7.0;
+ LaserScan_msg_.ranges[730+15]= 13.15;
+ LaserScan_msg_.ranges[730+20]= 13.15;
+ LaserScan_msg_.ranges[730+25]= 13.15;
+ LaserScan_msg_.ranges[730+30]= 13.15;
+ LaserScan_msg_.ranges[1300+18]= 7.0;
}
break;
@@ -373,24 +373,24 @@ void FakeLaserGenAlgNode::node_config_update(Config &config, uint32_t level)
LaserScan_msg_.ranges[1300]= 4.0;
LaserScan_msg_.ranges[1300+40]= 4.0;*/
- LaserScan_msg_.ranges[100]= 7.0;
- LaserScan_msg_.ranges[100+40]= 7.0;
+ LaserScan_msg_.ranges[685-20]= 13.15;
+ LaserScan_msg_.ranges[685-15]= 13.15;
//LaserScan_msg_.ranges[340]= 7.0;
//LaserScan_msg_.ranges[340+40]= 7.0;
- LaserScan_msg_.ranges[685-20]= 13.15;
- LaserScan_msg_.ranges[685-10]=13.15;
+ LaserScan_msg_.ranges[685-10]= 13.15;
+ LaserScan_msg_.ranges[685-5]=13.15;
// LaserScan_msg_.ranges[660]= 13.25;
// LaserScan_msg_.ranges[660+20]= 13.0;
// LaserScan_msg_.ranges[760]= 13.0;
// LaserScan_msg_.ranges[760+20]= 13.25;
LaserScan_msg_.ranges[685]= 13.15;
- LaserScan_msg_.ranges[685+10]= 13.0;
+ LaserScan_msg_.ranges[685+5]= 13.0;
LaserScan_msg_.ranges[725]= 13.0;
- LaserScan_msg_.ranges[725+10]= 13.15;
- LaserScan_msg_.ranges[725+20]= 13.15;
- LaserScan_msg_.ranges[725+30]= 13.15;
- //LaserScan_msg_.ranges[1060]= 7.0;
- //LaserScan_msg_.ranges[1060+40]= 7.0;
+ LaserScan_msg_.ranges[725+5]= 13.15;
+ LaserScan_msg_.ranges[725+10]= 13.15;
+ LaserScan_msg_.ranges[725+15]= 13.15;
+ LaserScan_msg_.ranges[725+20]= 13.15;
+ LaserScan_msg_.ranges[725+25]= 13.15;
LaserScan_msg_.ranges[1300]= 7.0;
LaserScan_msg_.ranges[1300+40]= 7.0;
}