From 0049f78f3d50b80522a9e5af871bd7a652dd7600 Mon Sep 17 00:00:00 2001
From: Ely Repiso Polo <erepiso@iri.upc.edu>
Date: Fri, 31 Mar 2017 17:26:06 +0000
Subject: [PATCH] akp planner node, simulate locompanion person that use akp
planner to meve
---
CMakeLists.txt | 37 ++++++++++++++++++--
launch/diff_platform_simulator.launch | 2 +-
launch/diff_platform_simulator2.launch | 15 ++++++++
launch/fake_laser_gen.launch | 2 +-
launch/fake_laser_gen2.launch | 12 +++++++
launch/fake_localization.launch | 4 +--
launch/map_server_local2.launch | 22 ++++++++++++
package.xml | 19 ++++++++--
src/fake_laser_gen_alg_node.cpp | 48 +++++++++++++-------------
9 files changed, 128 insertions(+), 33 deletions(-)
create mode 100644 launch/diff_platform_simulator2.launch
create mode 100644 launch/fake_laser_gen2.launch
create mode 100644 launch/map_server_local2.launch
diff --git a/CMakeLists.txt b/CMakeLists.txt
index cfb3fc6..19eaaa8 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 3fc3b29..4643c60 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 0000000..6e5c579
--- /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 e29d506..3ccc1fd 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 0000000..e03124e
--- /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 d6eac8e..cdfc056 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 0000000..46c65b8
--- /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 65fba47..0f479b7 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 fa278df..c21a260 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;
}
--
GitLab