Logical cam

CMakeLists.txt

@@ -1,30 +1,77 @@
 cmake_minimum_required(VERSION 3.0.2)
 project(aama_sim)
 
-## Compile as C++11, supported in ROS Kinetic and newer
-# add_compile_options(-std=c++11)
 
-## 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
-  gazebo_msgs
-  gazebo_ros
-  geometry_msgs
-  rospy
-  std_msgs
-  tf
-  turtlebot3_msgs
+    gazebo_ros
+    rospy
+    roscpp
+    std_msgs
+    sensor_msgs
+    geometry_msgs
+    gazebo_msgs
+    tf
+    turtlebot3_msgs
+    message_generation
 )
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
+find_package(gazebo REQUIRED)
 
+link_directories(${GAZEBO_LIBRARY_DIRS})
+
+include_directories(
+        include
+        lib
+        ${Boost_INCLUDE_DIR}
+        ${catkin_INCLUDE_DIRS}
+        ${GAZEBO_INCLUDE_DIRS}
+)
+
+add_library(logical_camera_plugin SHARED
+        src/logical_camera_plugin.cpp include/logical_camera_plugin.h
+        src/logical_camera_plugin.cpp
+        src/logical_camera_plugin.cpp
+)
+
+target_link_libraries(logical_camera_plugin
+        ${catkin_LIBRARIES}
+        ${GAZEBO_LIBRARIES}
+)
+
+add_message_files(
+        FILES
+        Model.msg
+        LogicalImage.msg
+)
 
 ## Uncomment this if the package has a setup.py. This macro ensures
 ## modules and global scripts declared therein get installed
 ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
 catkin_python_setup()
+generate_messages(
+        DEPENDENCIES
+        std_msgs
+        geometry_msgs
+        sensor_msgs
+)
+
+catkin_package(
+        INCLUDE_DIRS src include
+        CATKIN_DEPENDS
+        roscpp
+        rospy
+        sensor_msgs
+        std_msgs
+        message_runtime
+        gazebo_ros
+)
+catkin_install_python(PROGRAMS
+        src/aama_sim/simulation_controller/sensor_controller.py
+        src/aama_sim/simulation_controller/simulation_controller.py
+        DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
 
 ################################################
 ## Declare ROS messages, services and actions ##
@@ -106,53 +153,13 @@ catkin_python_setup()
 ## LIBRARIES: libraries you create in this project that dependent projects also need
 ## CATKIN_DEPENDS: catkin_packages dependent projects also need
 ## DEPENDS: system dependencies of this project that dependent projects also need
-catkin_package(
-#  INCLUDE_DIRS include
-#  LIBRARIES aamas_sim
-#  CATKIN_DEPENDS gazebo_msgs gazebo_ros geometry_msgs rospy std_msgs tf turtlebot3 turtlebot3_msgs turtlebot3_simulations
-#  DEPENDS system_lib
-)
+#catkin_package(
+##  INCLUDE_DIRS include
+##  LIBRARIES aamas_sim
+##  CATKIN_DEPENDS gazebo_msgs gazebo_ros geometry_msgs rospy std_msgs tf turtlebot3 turtlebot3_msgs turtlebot3_simulations
+##  DEPENDS system_lib
+#)
 
-###########
-## Build ##
-###########
-
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
-include_directories(
-# include
-  ${catkin_INCLUDE_DIRS}
-)
-
-## Declare a C++ library
-# add_library(${PROJECT_NAME}
-#   src/${PROJECT_NAME}/aamas_sim.cpp
-# )
-
-## Add cmake target dependencies of the library
-## as an example, code may need to be generated before libraries
-## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Declare a C++ executable
-## With catkin_make all packages are built within a single CMake context
-## The recommended prefix ensures that target names across packages don't collide
-# add_executable(${PROJECT_NAME}_node src/aamas_sim_node.cpp)
-
-## Rename C++ executable without prefix
-## The above recommended prefix causes long target names, the following renames the
-## target back to the shorter version for ease of user use
-## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
-# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
-
-## Add cmake target dependencies of the executable
-## same as for the library above
-# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Specify libraries to link a library or executable target against
-# target_link_libraries(${PROJECT_NAME}_node
-#   ${catkin_LIBRARIES}
-# )
 
 #############
 ## Install ##
@@ -163,11 +170,7 @@ include_directories(
 
 ## Mark executable scripts (Python etc.) for installation
 ## in contrast to setup.py, you can choose the destination
-catkin_install_python(PROGRAMS
-  src/aama_sim/simulation_controller/sensor_controller.py
-  src/aama_sim/simulation_controller/simulation_controller.py
-  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
+
 
 ## Mark executables for installation
 ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html

README.md

@@ -62,7 +62,18 @@ In order to run the Gazebo simulation with multiple robot run the commands below
   ```
    $ roslaunch aama_sim sim_controller.launch
   ```
-
+
+## Available Sensors
+
+| Sensor Name    | ROS Topic                           | RabbitMQ Topic   |
+|----------------|-------------------------------------|------------------|
+| IMU            | `{robot_name}/imu`                  | `imu`            |
+| Sonar          | `{robot_name}/sonar`                | `sonar`          |
+| UWB            | `{robot_name}/uwb`                  | `uwb`            |
+| Logical Camera | `{robot_name}/logical_camera_image` | `logical_camera` |
+| Camera         | Not Available                       | Not Available    |
+| LIDAR          | Not Available                       | Not Available    |
+
 ## Useful Links
 
 - [Example JADE Agents for AAMA-Sim](https://github.com/micss-lab/AAMA-example-agents)

include/logical_camera_plugin.h

@@ -0,0 +1,32 @@
+#pragma once
+
+#include <string>
+#include <sstream>
+
+#include <gazebo/common/Plugin.hh>
+#include <gazebo/gazebo.hh>
+#include <gazebo/rendering/rendering.hh>
+#include <gazebo/sensors/sensors.hh>
+
+#include <ros/ros.h>
+#include <std_msgs/String.h>
+
+#include <aama_sim/LogicalImage.h>
+
+namespace gazebo {
+    class LogicalCameraPlugin : public SensorPlugin{
+    public:
+        LogicalCameraPlugin() : SensorPlugin(){}
+        virtual ~LogicalCameraPlugin() {}
+
+        virtual void Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf);
+    protected:
+        ros::NodeHandle *nh;
+        ros::Publisher image_pub;
+    private:
+        virtual void OnUpdate();
+
+        sensors::LogicalCameraSensorPtr parentSensor;
+        event::ConnectionPtr updateConnection;
+    };
+}

msg/LogicalImage.msg

@@ -0,0 +1 @@
+Model[] models

msg/Model.msg

@@ -0,0 +1,2 @@
+string type
+geometry_msgs/Pose pose

package.xml

@@ -49,32 +49,39 @@
   <!-- Use doc_depend for packages you need only for building documentation: -->
   <!--   <doc_depend>doxygen</doc_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
-  <build_depend>gazebo_msgs</build_depend>
   <build_depend>gazebo_ros</build_depend>
-  <build_depend>geometry_msgs</build_depend>
+  <build_depend>roscpp</build_depend>
   <build_depend>rospy</build_depend>
   <build_depend>std_msgs</build_depend>
+  <build_depend>gazebo_msgs</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
   <build_depend>tf</build_depend>
   <build_depend>turtlebot3_msgs</build_depend>
+
   <build_export_depend>gazebo_msgs</build_export_depend>
   <build_export_depend>gazebo_ros</build_export_depend>
   <build_export_depend>geometry_msgs</build_export_depend>
   <build_export_depend>rospy</build_export_depend>
   <build_export_depend>std_msgs</build_export_depend>
   <build_export_depend>tf</build_export_depend>
   <build_export_depend>turtlebot3_msgs</build_export_depend>
+
   <exec_depend>gazebo_msgs</exec_depend>
   <exec_depend>gazebo_ros</exec_depend>
   <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>rospy</exec_depend>
   <exec_depend>std_msgs</exec_depend>
   <exec_depend>tf</exec_depend>
   <exec_depend>turtlebot3_msgs</exec_depend>
-
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>
     <!-- Other tools can request additional information be placed here -->
-
+    <gazebo_ros plugin_path="${prefix}/lib" gazebo_media_path="${prefix}" />
   </export>
 </package>

src/aama_sim/simulation_controller/sensor_controller.py

@@ -4,6 +4,7 @@
 from aama_sim.comm_interface.rabbitmq_interface import RabbitCommunication
 from aama_sim.simulation_interface.sensor.imu_interface import ImuInterface
 from aama_sim.simulation_interface.sensor.sonar_interface import SonarInterface
+from aama_sim.simulation_interface.sensor.logical_camera_interface import LogicalCameraInterface
 
 
 class SensorController:
@@ -18,12 +19,14 @@ def __init__(self):
 
         self.imu_interface = ImuInterface(self.robot_count)
         self.sonar_interface = SonarInterface(self.robot_count)
+        self.logical_camera_interface = LogicalCameraInterface(self.robot_count)
 
         self.rate = rospy.Rate(30)
 
     def fetch_sensor_msgs(self):
         self.imu_interface.send_imu_packet()
         self.sonar_interface.send_sonar_packet()
+        self.logical_camera_interface.send_logical_camera_packet()
 
     def run(self):
         while not rospy.is_shutdown():

src/aama_sim/simulation_interface/sensor/logical_camera_interface.py

@@ -0,0 +1,41 @@
+import rospy
+from aama_sim.comm_interface.rabbitmq_interface import RabbitCommunication
+from aama_sim.simulation_interface.sensor.logical_camera_robot_interface import LogicalCameraRobotInterface
+from sensor_msgs.msg import Imu
+
+
+class LogicalCameraInterface:
+
+    def __init__(self, robot_count):
+        self.robot_count = robot_count
+        self.robot_base_name = 'tb3_%s'
+
+        self.rabbitmq_interface = RabbitCommunication()
+        self.robot_logical_cameras = []
+
+        self.rabbit_queue_name = 'logical_camera'
+        self.rabbitmq_interface.register_queue(self.rabbit_queue_name)
+
+        self.init_robot_logical_camera_subs()
+
+    def init_robot_logical_camera_subs(self):
+        for i in range(self.robot_count):
+            robot_logical_camera = LogicalCameraRobotInterface(self.robot_base_name % i)
+            self.robot_logical_cameras.append(robot_logical_camera)
+
+    def send_logical_camera_packet(self):
+        logical_camera_packet = []
+        for logical_camera in self.robot_logical_cameras:
+            logical_camera_packet.append(logical_camera.logical_camera_msg)
+
+        self.rabbitmq_interface.send(self.rabbit_queue_name, logical_camera_packet)
+
+    def dry_run(self):
+        while not rospy.is_shutdown():
+            self.send_logical_camera_packet()
+
+
+# if __name__ == '__main__':
+#     rospy.init_node('test_imu')
+#     a = LogicalCameraInterface(3)
+#     a.dry_run()

src/aama_sim/simulation_interface/sensor/logical_camera_robot_interface.py

@@ -0,0 +1,21 @@
+import rospy
+
+from aama_sim.msg import LogicalImage
+from rospy_message_converter import message_converter
+
+
+class LogicalCameraRobotInterface:
+
+    def __init__(self, robot_name):
+        self.robot_name = robot_name
+
+        self.sub = rospy.Subscriber('/%s/logical_camera_image' % self.robot_name,
+                                    LogicalImage,
+                                    self.robot_logical_camera_callback,
+                                    queue_size=1)
+
+        self.logical_camera_msg = None
+
+    def robot_logical_camera_callback(self, data):
+        self.logical_camera_msg = message_converter.convert_ros_message_to_dictionary(data)
+        self.logical_camera_msg['robot_name'] = self.robot_name