diff --git a/CMakeLists.txt b/CMakeLists.txt index 5de0530..bb32339 100644 --- a/CMakeLists.txt +++ b/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 diff --git a/README.md b/README.md index 846d0d6..40b5de1 100644 --- a/README.md +++ b/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) diff --git a/include/logical_camera_plugin.h b/include/logical_camera_plugin.h new file mode 100644 index 0000000..0cfb4b4 --- /dev/null +++ b/include/logical_camera_plugin.h @@ -0,0 +1,32 @@ +#pragma once + +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include + +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; + }; +} \ No newline at end of file diff --git a/msg/LogicalImage.msg b/msg/LogicalImage.msg new file mode 100644 index 0000000..453738c --- /dev/null +++ b/msg/LogicalImage.msg @@ -0,0 +1 @@ +Model[] models \ No newline at end of file diff --git a/msg/Model.msg b/msg/Model.msg new file mode 100644 index 0000000..33da8c2 --- /dev/null +++ b/msg/Model.msg @@ -0,0 +1,2 @@ +string type +geometry_msgs/Pose pose \ No newline at end of file diff --git a/package.xml b/package.xml index 6277f4e..be7583c 100644 --- a/package.xml +++ b/package.xml @@ -49,13 +49,17 @@ catkin - gazebo_msgs gazebo_ros - geometry_msgs + roscpp rospy std_msgs + gazebo_msgs + sensor_msgs + geometry_msgs + message_generation tf turtlebot3_msgs + gazebo_msgs gazebo_ros geometry_msgs @@ -63,6 +67,7 @@ std_msgs tf turtlebot3_msgs + gazebo_msgs gazebo_ros geometry_msgs @@ -70,11 +75,13 @@ std_msgs tf turtlebot3_msgs - + roscpp + sensor_msgs + message_runtime - + diff --git a/src/aama_sim/simulation_controller/sensor_controller.py b/src/aama_sim/simulation_controller/sensor_controller.py index 4590a61..3f31cc1 100755 --- a/src/aama_sim/simulation_controller/sensor_controller.py +++ b/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(): diff --git a/src/aama_sim/simulation_interface/sensor/logical_camera_interface.py b/src/aama_sim/simulation_interface/sensor/logical_camera_interface.py new file mode 100644 index 0000000..635a8f4 --- /dev/null +++ b/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() diff --git a/src/aama_sim/simulation_interface/sensor/logical_camera_robot_interface.py b/src/aama_sim/simulation_interface/sensor/logical_camera_robot_interface.py new file mode 100644 index 0000000..4a071d9 --- /dev/null +++ b/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 diff --git a/src/logical_camera_plugin.cpp b/src/logical_camera_plugin.cpp new file mode 100644 index 0000000..d93288b --- /dev/null +++ b/src/logical_camera_plugin.cpp @@ -0,0 +1,74 @@ +#include "logical_camera_plugin.h" + +using namespace gazebo; +using namespace std; +using namespace ros; + +GZ_REGISTER_SENSOR_PLUGIN(LogicalCameraPlugin); + +void LogicalCameraPlugin::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf){ + // Get the parent sensor. + gazebo::sensors::SensorPtr genericSensor = _sensor; + this->parentSensor = std::dynamic_pointer_cast(_sensor); + + // Make sure the parent sensor is valid. + if (!this->parentSensor){ + gzerr << "LogicalCameraPlugin requires a LogicalCameraSensor.\n"; + return; + } + + // Connect to the sensor update event. + this->updateConnection = this->parentSensor->ConnectUpdated(std::bind(&LogicalCameraPlugin::OnUpdate, this)); + + // Make sure the parent sensor is active. + this->parentSensor->SetActive(true); + + ROS_INFO("LogicalCameraPlugin correctly loaded!!!"); + ROS_INFO("_near:=%g",this->parentSensor->Near()); + ROS_INFO("_far:=%g",this->parentSensor->Far()); + ROS_INFO("_horizontalFOV:=%g",this->parentSensor->HorizontalFOV()); + ROS_INFO("_aspect_ratio:=%g",this->parentSensor->AspectRatio()); + ROS_INFO("_asd:=%s",this->parentSensor->ParentName().c_str()); + + std::string s =this->parentSensor->ParentName(); + std::string delimiter = "::"; + std::string modelName = s.substr(0, s.find(delimiter)); + nh = new ros::NodeHandle(modelName); + image_pub = nh->advertise("logical_camera_image", 1, true); +} + +void LogicalCameraPlugin::OnUpdate(){ + msgs::LogicalCameraImage logical_image; + aama_sim::LogicalImage msg; + + logical_image = this->parentSensor->Image(); + gazebo::rendering::ScenePtr scene = gazebo::rendering::get_scene(); + + + int number_of_models = logical_image.model_size(); + for(int i=0; i < number_of_models; i++){ + aama_sim::Model model_msg; + + std::string modelName = logical_image.model(i).name(); + if (modelName != "ground_plane" && modelName != "lab_model") { + // Pose of camera in world coordinates + ignition::math::Pose3d cameraPose = gazebo::msgs::ConvertIgn(logical_image.pose()); + + // Pose of first model relative to camera + ignition::math::Pose3d modelRelativePose = gazebo::msgs::ConvertIgn(logical_image.model(i).pose()); + + // Pose of first model in world coordinates + ignition::math::Pose3d modelWorldPose = modelRelativePose + cameraPose; + + model_msg.pose.position.x = modelWorldPose.Pos().X(); + model_msg.pose.position.y = modelWorldPose.Pos().Y(); + model_msg.pose.position.z = modelWorldPose.Pos().Z(); + + model_msg.type = logical_image.model(i).name(); + + msg.models.push_back(model_msg); + } + } + + this->image_pub.publish(msg); +} \ No newline at end of file diff --git a/urdf/turtlebot3_burger.gazebo.xacro b/urdf/turtlebot3_burger.gazebo.xacro index 15f66c0..376cd19 100644 --- a/urdf/turtlebot3_burger.gazebo.xacro +++ b/urdf/turtlebot3_burger.gazebo.xacro @@ -168,6 +168,19 @@ 0.2967 + + + + 0.55 + 5.5 + 1.3962634 + 1 + + + true + true + 10 +