diff --git a/Dockerfile b/Dockerfile
index 37ca3ae4c1bcf41b3ca3a7e53431e55619645b0b..97249dc0b7a4e70eee35fdcfb25fce1b597517c0 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -1,133 +1,76 @@
+ARG RTABMAP=1
+ARG APRILTAGS=1
+ARG PROJECTIONS=1
+ARG SOCIALMPC=1
+
FROM ros:noetic-ros-base as ros-common
-ARG GPU=none
ENV TZ=Europe/Paris
RUN ln -snf /usr/share/zoneinfo/$TZ /etc/localtime && echo $TZ > /etc/timezone
RUN apt-get update && DEBIAN_FRONTEND=noninteractive apt-get install -y keyboard-configuration curl
-
RUN curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add -
-RUN apt-get update && apt-get install -y \
-ros-noetic-rosbridge-server \
-ros-noetic-controller-interface \
-ros-noetic-controller-manager \
-ros-noetic-controller-manager-msgs \
-ros-noetic-diff-drive-controller \
-ros-noetic-forward-command-controller \
-ros-noetic-joint-state-controller \
-ros-noetic-position-controllers \
-ros-noetic-rqt-* \
-ros-noetic-rviz \
-ros-noetic-teleop-* \
-ros-noetic-apriltag-*
-FROM ros-common as ros-rtabmap
+FROM ros-common as ros-rtabmap-0
+FROM ros-common as ros-rtabmap-1
RUN apt-get update && apt-get install -y \
ros-noetic-imu-tools \
ros-noetic-rtabmap-ros
-FROM ros-common as ros-social-mpc
-
-ENV NVIDIA_VISIBLE_DEVICES \
- ${NVIDIA_VISIBLE_DEVICES:-all}
-ENV NVIDIA_DRIVER_CAPABILITIES \
- ${NVIDIA_DRIVER_CAPABILITIES:+$NVIDIA_DRIVER_CAPABILITIES,}graphics
-
-RUN apt-get update && \
- apt-get -y install libgl1-mesa-glx libgl1-mesa-dri mesa-utils mesa-utils-extra && \
- rm -rf /var/lib/apt/lists/*
-
-RUN apt-get update && apt-get install -y python3-pip
-
-RUN mkdir -p /home/ros/robot_behavior_ws/src/robot_behavior/
-
-ADD ./action/ /home/ros/robot_behavior_ws/src/robot_behavior/action
-ADD ./config/ /home/ros/robot_behavior_ws/src/robot_behavior/config
-ADD ./launch/ /home/ros/robot_behavior_ws/src/robot_behavior/launch
-ADD ./modules/ /home/ros/robot_behavior_ws/src/robot_behavior/modules
-ADD ./msg/ /home/ros/robot_behavior_ws/src/robot_behavior/msg
-ADD ./src/ /home/ros/robot_behavior_ws/src/robot_behavior/src
-ADD ./CMakeLists.txt /home/ros/robot_behavior_ws/src/robot_behavior/
-ADD ./package.xml /home/ros/robot_behavior_ws/src/robot_behavior/
-ADD ./setup.py /home/ros/robot_behavior_ws/src/robot_behavior/
-
-
-RUN cd /home/ros/robot_behavior_ws/src/robot_behavior/modules/2D_Simulator && /bin/bash -c "pip install -r requirements.txt "
-RUN cd /home/ros/robot_behavior_ws/src/robot_behavior/modules/2D_Simulator && /bin/bash -c "sudo pip install -e ."
-RUN cd /home/ros/robot_behavior_ws/src/robot_behavior/modules/Motor_Controller && /bin/bash -c "pip install --force-reinstall -r requirements.txt"
-RUN cd /home/ros/robot_behavior_ws/src/robot_behavior/modules/Motor_Controller && /bin/bash -c "sudo pip install -e ."
-RUN cd /home/ros/robot_behavior_ws/ && /bin/bash -c "source /opt/ros/noetic/setup.bash && catkin_make"
-RUN /bin/bash -c "echo 'source /opt/ros/noetic/setup.bash' >> ~/.bashrc"
-RUN /bin/bash -c "echo 'source /home/ros/robot_behavior_ws/devel/setup.bash' >> ~/.bashrc"
-
-RUN rm /etc/ros/rosdep/sources.list.d/20-default.list
-RUN rosdep init
-RUN rosdep update
-RUN useradd ros
-RUN chown -R ros:ros /home/ros
-USER ros
-
-RUN /bin/bash -c "echo 'source /opt/ros/noetic/setup.bash' >> ~/.bashrc"
-RUN /bin/bash -c "echo 'source /home/ros/robot_behavior_ws/devel/setup.bash' >> ~/.bashrc"
-
-# Some QT-Apps/Gazebo don't show controls without this
-ENV QT_X11_NO_MITSHM 1
-ENV LIBGL_ALWAYS_INDIRECT=1
-
-FROM ros-social-mpc as ros-social-mpc-ari
-ARG ROS_IP=10.68.0.129
-ARG ROS_MASTER_URI=http://10.68.0.1:11311
-ENV ROS_IP ${ROS_IP}
-ENV ROS_MASTER_URI ${ROS_MASTER_URI}
-# setup entrypoint
-COPY ./scripts/entrypoint_social_mpc.sh /
-
-ENTRYPOINT ["/entrypoint_social_mpc.sh"]
-CMD ["teleop_joystick:=True", "plot_render:=True"]
-
-FROM ros-social-mpc as ros-social-mpc-sim
-# setup entrypoint
-COPY ./scripts/entrypoint_social_mpc_sim.sh /
-
-ENTRYPOINT ["/entrypoint_social_mpc_sim.sh"]
-CMD ["teleop_joystick:=True", "plot_render:=True"]
-
-FROM ros-rtabmap AS ros-rtabmap-perception
-RUN apt-get update && apt-get install -y ros-noetic-perception
-
-FROM ros-rtabmap-perception AS ros-emulated-inputs
+FROM ros-rtabmap-${RTABMAP} as ros-apriltags-0
+FROM ros-rtabmap-${RTABMAP} as ros-apriltags-1
RUN apt-get update && apt-get install -y \
-python3-pip \
-libv4l-dev \
-git
-RUN pip3 install wstool
+ros-noetic-apriltag-*
-RUN mkdir -p /home/ros/robot_behavior_ws/src/robot_behavior/config/apriltag/
-RUN cd /home/ros/robot_behavior_ws/src/ && catkin_create_pkg robot_behavior std_msgs rospy
-RUN mkdir -p /home/ros/robot_behavior_ws/src/robot_behavior/launch/
-RUN mkdir -p /home/ros/robot_behavior_ws/src/robot_behavior/maps/
-ADD /config/apriltag/ /home/ros/robot_behavior_ws/src/robot_behavior/config/apriltag/
-ADD /src/image_projection /home/ros/robot_behavior_ws/src/image_projection/
-ADD /launch/rtabmap_apriltag.launch /home/ros/robot_behavior_ws/src/robot_behavior/launch/rtabmap_apriltag.launch
-ADD /launch/stereo_rtabmap_inria.launch /home/ros/robot_behavior_ws/src/robot_behavior/launch/stereo_rtabmap_inria.launch
-ADD /launch/apriltag_continuous_detection.launch /home/ros/robot_behavior_ws/src/robot_behavior/launch/apriltag_continuous_detection.launch
-RUN /bin/bash -c "source /opt/ros/noetic/setup.bash && cd /home/ros/robot_behavior_ws/src/image_projection && rosdep update && rosdep install --from-paths . --ignore-src -r -y"
-RUN cd /home/ros/robot_behavior_ws/src/image_projection && wstool init ../.. && wstool merge image_projection_https.rosinstall && wstool update
+FROM ros-apriltags-${APRILTAGS} as ros-projections-0
+RUN mkdir -p /home/ros/robot_behavior_ws
+ADD src/robot_behavior /home/ros/robot_behavior_ws/src/robot_behavior
RUN cd /home/ros/robot_behavior_ws/ && /bin/bash -c "source /opt/ros/noetic/setup.bash && catkin_make"
-RUN /bin/bash -c "echo 'source /opt/ros/noetic/setup.bash' >> ~/.bashrc"
-RUN /bin/bash -c "echo 'source /home/ros/robot_behavior_ws/devel/setup.bash' >> ~/.bashrc"
+FROM ros-apriltags-${APRILTAGS} as ros-projections-1
+RUN mkdir -p /home/ros/projections_ws/
+RUN apt-get install -y python3-pip git libv4l-dev
+RUN pip3 install wstool
+ADD src/image_projection /home/ros/projections_ws/src/image_projection
RUN rm /etc/ros/rosdep/sources.list.d/20-default.list
RUN rosdep init
RUN rosdep update
+RUN rosdep install --from-paths /home/ros/projections_ws/src/image_projection --ignore-src -r -y
+RUN cd /home/ros/projections_ws/src/image_projection && wstool init ../.. && wstool merge image_projection_https.rosinstall && wstool update
+RUN cd /home/ros/projections_ws/ && /bin/bash -c "source /opt/ros/noetic/setup.bash && catkin_make && catkin_make install"
+
+FROM ros-projections-${PROJECTIONS} as ros-socialmpc-0
+FROM ros-projections-${PROJECTIONS} as ros-socialmpc-1
+
+RUN mkdir -p /home/ros/robot_behavior_ws
+ADD modules /home/ros/robot_behavior_ws/modules
+RUN cd /home/ros/robot_behavior_ws/modules/2D_Simulator && /bin/bash -c "pip install -r requirements.txt "
+RUN cd /home/ros/robot_behavior_ws/modules/2D_Simulator && /bin/bash -c "sudo pip install -e ."
+RUN cd /home/ros/robot_behavior_ws/modules/Motor_Controller && /bin/bash -c "pip install --force-reinstall -r requirements.txt"
+RUN cd /home/ros/robot_behavior_ws/modules/Motor_Controller && /bin/bash -c "sudo pip install -e ."
+# RUN cd /home/ros/robot_behavior_ws/ && /bin/bash -c "source /opt/ros/noetic/setup.bash && source /home/ros/projections_ws/devel/setup.bash || catkin_make"
+
+FROM ros-socialmpc-${SOCIALMPC} as wp6-proj-0-ws
+ADD src/robot_behavior /home/ros/robot_behavior_ws/src/robot_behavior
+WORKDIR /home/ros/robot_behavior_ws/
+RUN /bin/bash -c "source /opt/ros/noetic/setup.bash && catkin_make"
+FROM ros-socialmpc-${SOCIALMPC} as wp6-proj-1-ws
+ADD src/robot_behavior /home/ros/robot_behavior_ws/src/robot_behavior
+WORKDIR /home/ros/robot_behavior_ws/
+RUN /bin/bash -c "source /opt/ros/noetic/setup.bash && source /home/ros/projections_ws/devel/setup.bash && catkin_make"
+
+FROM wp6-proj-${PROJECTIONS}-ws as wp6-run
RUN useradd ros
RUN chown -R ros:ros /home/ros
+RUN chmod +x /home/ros/robot_behavior_ws/src/robot_behavior/*.py
USER ros
+COPY ./scripts/entrypoint.sh /
+ENTRYPOINT ["/entrypoint.sh", "bash"]
-RUN /bin/bash -c "echo 'source /opt/ros/noetic/setup.bash' >> ~/.bashrc"
-RUN /bin/bash -c "echo 'source /home/ros/robot_behavior_ws/devel/setup.bash' >> ~/.bashrc"
-
-# setup entrypoint
-COPY ./scripts/entrypoint_emulated_inputs.sh /
-
-ENTRYPOINT ["/entrypoint_emulated_inputs.sh"]
+FROM wp6-run as wp6-rtabmap
+ENTRYPOINT ["/entrypoint.sh", "roslaunch", "robot_behavior", "stereo_rtabmap_inria.launch"]
CMD ["localization:=True", "database_name:=inria_office_stereo_6"]
+
+FROM wp6-run as wp6-demo-peirasmos
+#ENTRYPOINT ["bash"]
+ENTRYPOINT ["/entrypoint.sh"]
+CMD ["roslaunch", "robot_behavior", "wp6_demo_peirasmos.launch"]
\ No newline at end of file
diff --git a/scripts/entrypoint.sh b/scripts/entrypoint.sh
new file mode 100755
index 0000000000000000000000000000000000000000..951f14362dfab6fe7a5bdd57bc282145643cac74
--- /dev/null
+++ b/scripts/entrypoint.sh
@@ -0,0 +1,8 @@
+#!/bin/bash
+set -e
+
+# setup ros environment
+echo $@
+source /opt/ros/noetic/setup.bash
+source /home/ros/robot_behavior_ws/devel/setup.bash
+exec $@
diff --git a/CMakeLists.txt b/src/robot_behavior/CMakeLists.txt
similarity index 79%
rename from CMakeLists.txt
rename to src/robot_behavior/CMakeLists.txt
index d9cc3e521a4831b39f4c39a62074da97e941a617..6f8568ab6d81d7598b3860955c8ffd1048cabab9 100644
--- a/CMakeLists.txt
+++ b/src/robot_behavior/CMakeLists.txt
@@ -175,30 +175,30 @@ include_directories(
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
-catkin_install_python(PROGRAMS src/robot_behavior/sim_2d_main.py
- src/robot_behavior/sim_2d_node.py
- src/robot_behavior/controller_main.py
- src/robot_behavior/controller_node.py
- src/robot_behavior/mpc_action_server.py
- src/robot_behavior/mpc_action_client.py
- src/robot_behavior/mpc_action_server_main.py
- src/robot_behavior/mpc_action_client_main.py
- src/robot_behavior/navigate_action_server.py
- src/robot_behavior/navigate_action_client.py
- src/robot_behavior/navigate_action_server_main.py
- src/robot_behavior/navigate_action_client_main.py
- src/robot_behavior/gotowards_action_server.py
- src/robot_behavior/gotowards_action_client.py
- src/robot_behavior/gotowards_action_server_main.py
- src/robot_behavior/gotowards_action_client_main.py
- src/robot_behavior/lookat_action_server.py
- src/robot_behavior/lookat_action_client.py
- src/robot_behavior/lookat_action_server_main.py
- src/robot_behavior/lookat_action_client_main.py
- src/robot_behavior/republish_from_apriltag_main.py
- src/robot_behavior/republish_from_rtabmap_main.py
- src/robot_behavior/republish_from_apriltag_node.py
- src/robot_behavior/republish_from_rtabmap_node.py
+catkin_install_python(PROGRAMS scripts/sim_2d_main.py
+ scripts/sim_2d_node.py
+ scripts/controller_main.py
+ scripts/controller_node.py
+ scripts/mpc_action_server.py
+ scripts/mpc_action_client.py
+ scripts/mpc_action_server_main.py
+ scripts/mpc_action_client_main.py
+ scripts/navigate_action_server.py
+ scripts/navigate_action_client.py
+ scripts/navigate_action_server_main.py
+ scripts/navigate_action_client_main.py
+ scripts/gotowards_action_server.py
+ scripts/gotowards_action_client.py
+ scripts/gotowards_action_server_main.py
+ scripts/gotowards_action_client_main.py
+ scripts/lookat_action_server.py
+ scripts/lookat_action_client.py
+ scripts/lookat_action_server_main.py
+ scripts/lookat_action_client_main.py
+ scripts/republish_from_apriltag_main.py
+ scripts/republish_from_rtabmap_main.py
+ scripts/republish_from_apriltag_node.py
+ scripts/republish_from_rtabmap_node.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
diff --git a/action/GoTowards.action b/src/robot_behavior/action/GoTowards.action
similarity index 100%
rename from action/GoTowards.action
rename to src/robot_behavior/action/GoTowards.action
diff --git a/action/LookAt.action b/src/robot_behavior/action/LookAt.action
similarity index 100%
rename from action/LookAt.action
rename to src/robot_behavior/action/LookAt.action
diff --git a/action/MPC.action b/src/robot_behavior/action/MPC.action
similarity index 100%
rename from action/MPC.action
rename to src/robot_behavior/action/MPC.action
diff --git a/action/Navigate.action b/src/robot_behavior/action/Navigate.action
similarity index 100%
rename from action/Navigate.action
rename to src/robot_behavior/action/Navigate.action
diff --git a/config/apriltag/settings.yaml b/src/robot_behavior/config/apriltag/settings.yaml
similarity index 100%
rename from config/apriltag/settings.yaml
rename to src/robot_behavior/config/apriltag/settings.yaml
diff --git a/config/apriltag/tags.yaml b/src/robot_behavior/config/apriltag/tags.yaml
similarity index 100%
rename from config/apriltag/tags.yaml
rename to src/robot_behavior/config/apriltag/tags.yaml
diff --git a/src/robot_behavior/config/projections/fisheye_calibration.yaml b/src/robot_behavior/config/projections/fisheye_calibration.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..109b38af2c34a9e8e309f02fc59160be996f860d
--- /dev/null
+++ b/src/robot_behavior/config/projections/fisheye_calibration.yaml
@@ -0,0 +1,7 @@
+camera_name: cam
+resolution: [1280, 960]
+camera_model: omni
+intrinsics: [2.79280406, 1729.25939219, 1729.12908599, 637.01968823, 482.72441258]
+distortion_model: radtan
+distortion_coeffs: [-0.09805262, 0.52415826, -0.00283125, -0.00240354]
+mask_path: $(find image_projection_demo)/config/insta360_3_omni_radtan_calibration/left_mask.png
diff --git a/src/robot_behavior/config/projections/front_fisheye_camera.yaml b/src/robot_behavior/config/projections/front_fisheye_camera.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..56d363eef98a15962801b62f0ce48c0d551058df
--- /dev/null
+++ b/src/robot_behavior/config/projections/front_fisheye_camera.yaml
@@ -0,0 +1,9 @@
+# Used cameras for the projection
+cameras:
+ - '/front_camera/fisheye/'
+
+# Topic setup
+/front_camera/fisheye/:
+ image_topic: image_raw
+ camera_info_topic: camera_info_not_used
+ extended_camera_info_topic: extended_camera_info
diff --git a/src/robot_behavior/config/projections/projections_inria.yaml b/src/robot_behavior/config/projections/projections_inria.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..bfaac3efd73297a3077a74473e7def578bf14bf3
--- /dev/null
+++ b/src/robot_behavior/config/projections/projections_inria.yaml
@@ -0,0 +1,14 @@
+# Front view pinhole projection
+front_fisheye_camera_rect:
+ update_rate: 10
+ always_recompute_mapping: false
+ base_frame: front_fisheye_camera_link
+ pose: [0, 0, 0, 0, 0.4, 0]
+ virtual_sensor_frame: pinhole_front_camera_frame
+ virtual_sensor_optical_frame: pinhole_front_optical_frame
+ projection_type: image_projection_plugins::PinholeProjection
+ projection_parameters:
+ image_width: 1024
+ image_height: 512
+ horizontal_fov: 130
+ focal_length: 1
diff --git a/config/rviz/apriltag.rviz b/src/robot_behavior/config/rviz/apriltag.rviz
similarity index 100%
rename from config/rviz/apriltag.rviz
rename to src/robot_behavior/config/rviz/apriltag.rviz
diff --git a/config/rviz/localization_apriltag.rviz b/src/robot_behavior/config/rviz/localization_apriltag.rviz
similarity index 100%
rename from config/rviz/localization_apriltag.rviz
rename to src/robot_behavior/config/rviz/localization_apriltag.rviz
diff --git a/config/rviz/mapping_localization.rviz b/src/robot_behavior/config/rviz/mapping_localization.rviz
similarity index 100%
rename from config/rviz/mapping_localization.rviz
rename to src/robot_behavior/config/rviz/mapping_localization.rviz
diff --git a/config/rviz/social_mpc_navigation.rviz b/src/robot_behavior/config/rviz/social_mpc_navigation.rviz
similarity index 100%
rename from config/rviz/social_mpc_navigation.rviz
rename to src/robot_behavior/config/rviz/social_mpc_navigation.rviz
diff --git a/config/social_mpc/robot.yaml b/src/robot_behavior/config/social_mpc/robot.yaml
similarity index 100%
rename from config/social_mpc/robot.yaml
rename to src/robot_behavior/config/social_mpc/robot.yaml
diff --git a/config/social_mpc/social_mpc.yaml b/src/robot_behavior/config/social_mpc/social_mpc.yaml
similarity index 100%
rename from config/social_mpc/social_mpc.yaml
rename to src/robot_behavior/config/social_mpc/social_mpc.yaml
diff --git a/launch/apriltag2tracking.launch b/src/robot_behavior/launch/apriltag2tracking.launch
similarity index 100%
rename from launch/apriltag2tracking.launch
rename to src/robot_behavior/launch/apriltag2tracking.launch
diff --git a/launch/apriltag2tracking_social_mpc_action_servers.launch b/src/robot_behavior/launch/apriltag2tracking_social_mpc_action_servers.launch
similarity index 100%
rename from launch/apriltag2tracking_social_mpc_action_servers.launch
rename to src/robot_behavior/launch/apriltag2tracking_social_mpc_action_servers.launch
diff --git a/launch/apriltag_continuous_detection.launch b/src/robot_behavior/launch/apriltag_continuous_detection.launch
similarity index 100%
rename from launch/apriltag_continuous_detection.launch
rename to src/robot_behavior/launch/apriltag_continuous_detection.launch
diff --git a/launch/gotowards_action_client_launch.launch b/src/robot_behavior/launch/gotowards_action_client_launch.launch
similarity index 100%
rename from launch/gotowards_action_client_launch.launch
rename to src/robot_behavior/launch/gotowards_action_client_launch.launch
diff --git a/src/robot_behavior/launch/image_rectification.launch b/src/robot_behavior/launch/image_rectification.launch
new file mode 100644
index 0000000000000000000000000000000000000000..036809f9ac0278c87da79938c739c44ba9301cf5
--- /dev/null
+++ b/src/robot_behavior/launch/image_rectification.launch
@@ -0,0 +1,7 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+<arg name="front_fisheye_camera_ns" default="/front_fisheye_basestation"/>
+<include file="$(find image_projection_demo)/launch/projections_inria.launch">
+ <arg name="camera_ns" value="$(arg front_fisheye_camera_ns)"/>
+</include>
+</launch>
\ No newline at end of file
diff --git a/launch/lookat_action_client_launch.launch b/src/robot_behavior/launch/lookat_action_client_launch.launch
similarity index 100%
rename from launch/lookat_action_client_launch.launch
rename to src/robot_behavior/launch/lookat_action_client_launch.launch
diff --git a/launch/mpc_action_client_launch.launch b/src/robot_behavior/launch/mpc_action_client_launch.launch
similarity index 100%
rename from launch/mpc_action_client_launch.launch
rename to src/robot_behavior/launch/mpc_action_client_launch.launch
diff --git a/launch/mpc_action_server_sim_launch.launch b/src/robot_behavior/launch/mpc_action_server_sim_launch.launch
similarity index 100%
rename from launch/mpc_action_server_sim_launch.launch
rename to src/robot_behavior/launch/mpc_action_server_sim_launch.launch
diff --git a/launch/navigate_action_client_launch.launch b/src/robot_behavior/launch/navigate_action_client_launch.launch
similarity index 100%
rename from launch/navigate_action_client_launch.launch
rename to src/robot_behavior/launch/navigate_action_client_launch.launch
diff --git a/launch/rtabmap_apriltag.launch b/src/robot_behavior/launch/rtabmap_apriltag.launch
similarity index 100%
rename from launch/rtabmap_apriltag.launch
rename to src/robot_behavior/launch/rtabmap_apriltag.launch
diff --git a/launch/rtabmap_apriltag_social_mpc.launch b/src/robot_behavior/launch/rtabmap_apriltag_social_mpc.launch
similarity index 100%
rename from launch/rtabmap_apriltag_social_mpc.launch
rename to src/robot_behavior/launch/rtabmap_apriltag_social_mpc.launch
diff --git a/launch/social_mpc_sim.launch b/src/robot_behavior/launch/social_mpc_sim.launch
similarity index 100%
rename from launch/social_mpc_sim.launch
rename to src/robot_behavior/launch/social_mpc_sim.launch
diff --git a/launch/social_mpc_sim_action_servers.launch b/src/robot_behavior/launch/social_mpc_sim_action_servers.launch
similarity index 100%
rename from launch/social_mpc_sim_action_servers.launch
rename to src/robot_behavior/launch/social_mpc_sim_action_servers.launch
diff --git a/launch/stereo_rtabmap_inria.launch b/src/robot_behavior/launch/stereo_rtabmap_inria.launch
similarity index 100%
rename from launch/stereo_rtabmap_inria.launch
rename to src/robot_behavior/launch/stereo_rtabmap_inria.launch
diff --git a/src/robot_behavior/launch/wp6_demo_peirasmos.launch b/src/robot_behavior/launch/wp6_demo_peirasmos.launch
new file mode 100644
index 0000000000000000000000000000000000000000..333317c70e67f9f79a5f27361b3ef39288171690
--- /dev/null
+++ b/src/robot_behavior/launch/wp6_demo_peirasmos.launch
@@ -0,0 +1,134 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+ <arg name="teleop_keyboard" default="False"/>
+ <arg name="teleop_joystick" default="True"/>
+ <arg name="republish_from_rtabmap" default="True"/>
+ <arg name="republish_from_apriltag" default="True"/>
+ <arg name="social_mpc" default="True"/>
+ <arg name="action_servers" default="True"/>
+ <arg name="rviz" default="False"/>
+
+ <arg name="mobile_base_controller" default="/mobile_base_controller"/>
+ <arg name="speed" default="0.3"/>
+ <arg name="turn" default="0.5"/>
+
+ <arg name="rviz_config_file" default="localization_apriltag"/>
+
+ <arg name="reference_frame" default="base_footprint"/>
+ <arg name="cam_optical_frame" default="pinhole_front_optical_frame"/>
+
+ <arg name="mpc_config_name" default="social_mpc"/>
+ <arg name="robot_config_name" default="robot"/>
+ <arg name="pub_hz_control" default="20"/>
+ <arg name="plot_render" default="False"/>
+ <arg name="reference_map" default="rtabmap_map"/>
+ <arg name="max_humans_world" default="20"/>
+
+ <arg name="success_dist_thresh" default="0.25"/>
+ <arg name="success_ang_thresh" default="0.2"/>
+
+ <arg name="camera_ns" default="/front_camera/fisheye"/>
+ <arg name="in_topic_republish" default="$(arg camera_ns)/image_raw"/>
+ <arg name="out_topic_republish" default="$(arg camera_ns)/image_raw"/>
+ <arg name="fisheye_camera_frame" default="front_fisheye_camera_optical_frame"/>
+
+ <arg name="rect_camera_ns" default="$(arg camera_ns)/rect"/>
+
+
+ <node pkg="image_transport" type="republish" name="republish" output="screen" args="compressed in:=$(arg in_topic_republish) raw out:=$(arg out_topic_republish)" />
+
+ <node name="camera_info_publisher" pkg="kalibr_extended_camera_info_publisher" type="camera_info_publisher_node">
+ <param name="camera_ns" value="$(arg camera_ns)"/>
+ <param name="frame_id" value="$(arg fisheye_camera_frame)"/>
+ <rosparam file="$(find robot_behavior)/config/projections/fisheye_calibration.yaml" command="load" subst_value="true" />
+ </node>
+
+ <rosparam command="load" file="$(find robot_behavior)/config/projections/projections_inria.yaml" subst_value="true"/>
+
+ <node name="front_fisheye_camera_rect" pkg="image_projection" type="periodic_image_projection_node">
+ <rosparam command="load" file="$(find robot_behavior)/config/projections/front_fisheye_camera.yaml" subst_value="true"/>
+ <remap from="~projection" to="$(arg camera_ns)/rect/image_raw" />
+ <remap from="~camera_info" to="$(arg camera_ns)/rect/camera_info" />
+ </node>
+
+
+ <node pkg="apriltag_ros" type="apriltag_ros_continuous_node" name="apriltags" clear_params="true" output="screen">
+ <!-- Set parameters -->
+ <rosparam command="load" file="$(find robot_behavior)/config/apriltag/settings.yaml"/>
+ <rosparam command="load" file="$(find robot_behavior)/config/apriltag/tags.yaml"/>
+ <!-- Remap topics from those used in code to those on the ROS network -->
+ <remap from="image_rect" to="$(arg camera_ns)/rect/image_raw" />
+ <remap from="camera_info" to="$(arg camera_ns)/rect/camera_info" />
+
+ <param name="publish_tag_detections_image" type="bool" value="true" />
+ <param name="camera_frame" type="str" value="pinhole_front_camera_frame" />
+ </node>
+
+ <!--
+ <group if="$(arg action_servers)">
+ <node pkg="robot_behavior" type="navigate_action_server_main.py" name="navigate_action_server_main" output="screen">
+ <param name="hz" type="int" value="$(arg pub_hz_control)"/>
+ <param name="success_dist_thresh" type="double" value="$(arg success_dist_thresh)"/>
+ <remap from="odom" to="rtabmap_odom_rectified"/>
+ </node>
+
+ <node pkg="robot_behavior" type="gotowards_action_server_main.py" name="gotowards_action_server_main" output="screen">
+ <param name="hz" type="int" value="$(arg pub_hz_control)"/>
+ <param name="success_dist_thresh" type="double" value="$(arg success_dist_thresh)"/>
+ <remap from="odom" to="rtabmap_odom_rectified"/>
+ </node>
+
+ <node pkg="robot_behavior" type="lookat_action_server_main.py" name="lookat_action_server_main" output="screen">
+ <param name="hz" type="int" value="$(arg pub_hz_control)"/>
+ <param name="success_ang_thresh" type="double" value="$(arg success_ang_thresh)"/>
+ <remap from="odom" to="rtabmap_odom_rectified"/>
+ </node>
+ </group>
+
+ <group if="$(arg social_mpc)">
+ <node pkg="robot_behavior" type="controller_main.py" name="social_mpc" output="screen">
+ <param name="mpc_config_name" type="str" value="$(find robot_behavior)/config/social_mpc/$(arg mpc_config_name).yaml"/>
+ <param name="robot_config_name" type="str" value="$(find robot_behavior)/config/social_mpc/$(arg robot_config_name).yaml"/>
+ <param name="plot_render" type="bool" value="$(arg plot_render)"/>
+ <param name="reference_map" type="str" value="$(arg reference_map)"/>
+ <param name="max_humans_world" type="int" value="$(arg max_humans_world)"/>
+ <remap from="odom" to="rtabmap_odom_rectified"/>
+ <remap from="local_map" to="rtabmap_local_map_rectified"/>
+ <remap from="global_map" to="/rtabmap/proj_map"/>
+ <remap from="cmd_vel" to="/nav_vel"/>
+ </node>
+ </group>
+
+ <group if="$(arg republish_from_apriltag)">
+ <node pkg="robot_behavior" type="republish_from_apriltag_main.py" name="republish_from_apriltag_main" output="screen">
+ <param name="reference_frame" type="str" value="$(arg reference_frame)"/>
+ <param name="cam_optical_frame" type="str" value="$(arg cam_optical_frame)"/>
+ </node>
+ </group>
+
+ <group if="$(arg republish_from_rtabmap)">
+ <node pkg="robot_behavior" type="republish_from_rtabmap_main.py" name="republish_from_rtabmap_main" output="screen"/>
+ </group>
+
+ <group if="$(arg teleop_keyboard)">
+ <node pkg="teleop_twist_keyboard" type="teleop_twist_keyboard.py" name="teleop_twist_keyboard" output="screen" >
+ <remap from="cmd_vel" to="$(arg mobile_base_controller)/cmd_vel" />
+ <param name="~speed" type="double" value="$(arg speed)" />
+ <param name="~turn" type="double" value="$(arg turn)" />
+ </node>
+ </group>
+ -->
+ <!--
+ <group if="$(arg teleop_joystick)">
+ <node pkg="joy" type="joy_node" name="joystick">
+ <param name="autorepeat_rate" value="10" />
+ <remap from="diagnostics" to="joystick_diagnostics"/>
+ </node>
+ </group>
+ -->
+
+ <group if="$(arg rviz)">
+ <node type="rviz" name="rviz" pkg="rviz" args="-d $(find robot_behavior)/config/rviz/$(arg rviz_config_file).rviz" />
+ </group>
+
+</launch>
diff --git a/msg/ControllerStatus.msg b/src/robot_behavior/msg/ControllerStatus.msg
similarity index 100%
rename from msg/ControllerStatus.msg
rename to src/robot_behavior/msg/ControllerStatus.msg
diff --git a/msg/GoTowards.msg b/src/robot_behavior/msg/GoTowards.msg
similarity index 100%
rename from msg/GoTowards.msg
rename to src/robot_behavior/msg/GoTowards.msg
diff --git a/msg/LookAt.msg b/src/robot_behavior/msg/LookAt.msg
similarity index 100%
rename from msg/LookAt.msg
rename to src/robot_behavior/msg/LookAt.msg
diff --git a/msg/Navigate.msg b/src/robot_behavior/msg/Navigate.msg
similarity index 100%
rename from msg/Navigate.msg
rename to src/robot_behavior/msg/Navigate.msg
diff --git a/msg/TrackedPerson2d.msg b/src/robot_behavior/msg/TrackedPerson2d.msg
similarity index 100%
rename from msg/TrackedPerson2d.msg
rename to src/robot_behavior/msg/TrackedPerson2d.msg
diff --git a/msg/TrackedPersons2d.msg b/src/robot_behavior/msg/TrackedPersons2d.msg
similarity index 100%
rename from msg/TrackedPersons2d.msg
rename to src/robot_behavior/msg/TrackedPersons2d.msg
diff --git a/package.xml b/src/robot_behavior/package.xml
similarity index 100%
rename from package.xml
rename to src/robot_behavior/package.xml
diff --git a/src/robot_behavior/__init__.py b/src/robot_behavior/scripts/__init__.py
old mode 100644
new mode 100755
similarity index 100%
rename from src/robot_behavior/__init__.py
rename to src/robot_behavior/scripts/__init__.py
diff --git a/src/robot_behavior/controller_main.py b/src/robot_behavior/scripts/controller_main.py
old mode 100644
new mode 100755
similarity index 100%
rename from src/robot_behavior/controller_main.py
rename to src/robot_behavior/scripts/controller_main.py
diff --git a/src/robot_behavior/controller_node.py b/src/robot_behavior/scripts/controller_node.py
old mode 100644
new mode 100755
similarity index 100%
rename from src/robot_behavior/controller_node.py
rename to src/robot_behavior/scripts/controller_node.py
diff --git a/src/robot_behavior/gotowards_action_client.py b/src/robot_behavior/scripts/gotowards_action_client.py
old mode 100644
new mode 100755
similarity index 100%
rename from src/robot_behavior/gotowards_action_client.py
rename to src/robot_behavior/scripts/gotowards_action_client.py
diff --git a/src/robot_behavior/gotowards_action_client_main.py b/src/robot_behavior/scripts/gotowards_action_client_main.py
old mode 100644
new mode 100755
similarity index 100%
rename from src/robot_behavior/gotowards_action_client_main.py
rename to src/robot_behavior/scripts/gotowards_action_client_main.py
diff --git a/src/robot_behavior/gotowards_action_server.py b/src/robot_behavior/scripts/gotowards_action_server.py
old mode 100644
new mode 100755
similarity index 100%
rename from src/robot_behavior/gotowards_action_server.py
rename to src/robot_behavior/scripts/gotowards_action_server.py
diff --git a/src/robot_behavior/gotowards_action_server_main.py b/src/robot_behavior/scripts/gotowards_action_server_main.py
old mode 100644
new mode 100755
similarity index 100%
rename from src/robot_behavior/gotowards_action_server_main.py
rename to src/robot_behavior/scripts/gotowards_action_server_main.py
diff --git a/src/robot_behavior/lookat_action_client.py b/src/robot_behavior/scripts/lookat_action_client.py
old mode 100644
new mode 100755
similarity index 100%
rename from src/robot_behavior/lookat_action_client.py
rename to src/robot_behavior/scripts/lookat_action_client.py
diff --git a/src/robot_behavior/lookat_action_client_main.py b/src/robot_behavior/scripts/lookat_action_client_main.py
old mode 100644
new mode 100755
similarity index 100%
rename from src/robot_behavior/lookat_action_client_main.py
rename to src/robot_behavior/scripts/lookat_action_client_main.py
diff --git a/src/robot_behavior/lookat_action_server.py b/src/robot_behavior/scripts/lookat_action_server.py
old mode 100644
new mode 100755
similarity index 100%
rename from src/robot_behavior/lookat_action_server.py
rename to src/robot_behavior/scripts/lookat_action_server.py
diff --git a/src/robot_behavior/lookat_action_server_main.py b/src/robot_behavior/scripts/lookat_action_server_main.py
old mode 100644
new mode 100755
similarity index 100%
rename from src/robot_behavior/lookat_action_server_main.py
rename to src/robot_behavior/scripts/lookat_action_server_main.py
diff --git a/src/robot_behavior/mpc_action_client.py b/src/robot_behavior/scripts/mpc_action_client.py
old mode 100644
new mode 100755
similarity index 100%
rename from src/robot_behavior/mpc_action_client.py
rename to src/robot_behavior/scripts/mpc_action_client.py
diff --git a/src/robot_behavior/mpc_action_client_main.py b/src/robot_behavior/scripts/mpc_action_client_main.py
old mode 100644
new mode 100755
similarity index 100%
rename from src/robot_behavior/mpc_action_client_main.py
rename to src/robot_behavior/scripts/mpc_action_client_main.py
diff --git a/src/robot_behavior/mpc_action_server.py b/src/robot_behavior/scripts/mpc_action_server.py
old mode 100644
new mode 100755
similarity index 100%
rename from src/robot_behavior/mpc_action_server.py
rename to src/robot_behavior/scripts/mpc_action_server.py
diff --git a/src/robot_behavior/mpc_action_server_main.py b/src/robot_behavior/scripts/mpc_action_server_main.py
old mode 100644
new mode 100755
similarity index 100%
rename from src/robot_behavior/mpc_action_server_main.py
rename to src/robot_behavior/scripts/mpc_action_server_main.py
diff --git a/src/robot_behavior/navigate_action_client.py b/src/robot_behavior/scripts/navigate_action_client.py
old mode 100644
new mode 100755
similarity index 100%
rename from src/robot_behavior/navigate_action_client.py
rename to src/robot_behavior/scripts/navigate_action_client.py
diff --git a/src/robot_behavior/navigate_action_client_main.py b/src/robot_behavior/scripts/navigate_action_client_main.py
old mode 100644
new mode 100755
similarity index 100%
rename from src/robot_behavior/navigate_action_client_main.py
rename to src/robot_behavior/scripts/navigate_action_client_main.py
diff --git a/src/robot_behavior/navigate_action_server.py b/src/robot_behavior/scripts/navigate_action_server.py
old mode 100644
new mode 100755
similarity index 100%
rename from src/robot_behavior/navigate_action_server.py
rename to src/robot_behavior/scripts/navigate_action_server.py
diff --git a/src/robot_behavior/navigate_action_server_main.py b/src/robot_behavior/scripts/navigate_action_server_main.py
old mode 100644
new mode 100755
similarity index 100%
rename from src/robot_behavior/navigate_action_server_main.py
rename to src/robot_behavior/scripts/navigate_action_server_main.py
diff --git a/src/robot_behavior/republish_from_apriltag_main.py b/src/robot_behavior/scripts/republish_from_apriltag_main.py
old mode 100644
new mode 100755
similarity index 100%
rename from src/robot_behavior/republish_from_apriltag_main.py
rename to src/robot_behavior/scripts/republish_from_apriltag_main.py
diff --git a/src/robot_behavior/republish_from_apriltag_node.py b/src/robot_behavior/scripts/republish_from_apriltag_node.py
old mode 100644
new mode 100755
similarity index 100%
rename from src/robot_behavior/republish_from_apriltag_node.py
rename to src/robot_behavior/scripts/republish_from_apriltag_node.py
diff --git a/src/robot_behavior/republish_from_rtabmap_main.py b/src/robot_behavior/scripts/republish_from_rtabmap_main.py
old mode 100644
new mode 100755
similarity index 100%
rename from src/robot_behavior/republish_from_rtabmap_main.py
rename to src/robot_behavior/scripts/republish_from_rtabmap_main.py
diff --git a/src/robot_behavior/republish_from_rtabmap_node.py b/src/robot_behavior/scripts/republish_from_rtabmap_node.py
old mode 100644
new mode 100755
similarity index 100%
rename from src/robot_behavior/republish_from_rtabmap_node.py
rename to src/robot_behavior/scripts/republish_from_rtabmap_node.py
diff --git a/src/robot_behavior/sim_2d_main.py b/src/robot_behavior/scripts/sim_2d_main.py
old mode 100644
new mode 100755
similarity index 100%
rename from src/robot_behavior/sim_2d_main.py
rename to src/robot_behavior/scripts/sim_2d_main.py
diff --git a/src/robot_behavior/sim_2d_node.py b/src/robot_behavior/scripts/sim_2d_node.py
similarity index 100%
rename from src/robot_behavior/sim_2d_node.py
rename to src/robot_behavior/scripts/sim_2d_node.py
diff --git a/src/robot_behavior/utils.py b/src/robot_behavior/scripts/utils.py
old mode 100644
new mode 100755
similarity index 100%
rename from src/robot_behavior/utils.py
rename to src/robot_behavior/scripts/utils.py
diff --git a/setup.py b/src/robot_behavior/setup.py
old mode 100644
new mode 100755
similarity index 100%
rename from setup.py
rename to src/robot_behavior/setup.py