diff --git a/ca_gazebo/CMakeLists.txt b/ca_gazebo/CMakeLists.txt index 7d909fa74..c4b4a8a3c 100644 --- a/ca_gazebo/CMakeLists.txt +++ b/ca_gazebo/CMakeLists.txt @@ -30,11 +30,15 @@ else() ${GAZEBO_INCLUDE_DIRS} ) - add_library(create_bumper_plugin src/create_bumper_plugin.cpp) + add_library(create_bumper_plugin src/create_bumper_plugin.cpp) add_dependencies(create_bumper_plugin ${catkin_EXPORTED_TARGETS}) target_link_libraries(create_bumper_plugin ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES}) + add_library(model_pose_publisher_plugin src/model_pose_publisher_plugin.cpp) + target_link_libraries(model_pose_publisher_plugin + ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES}) + install(DIRECTORY launch worlds models DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/ca_gazebo/launch/create_empty_world.launch b/ca_gazebo/launch/create_empty_world.launch index 79bb84d68..e9a05ecf2 100644 --- a/ca_gazebo/launch/create_empty_world.launch +++ b/ca_gazebo/launch/create_empty_world.launch @@ -1,20 +1,21 @@ - - + + + - + - + @@ -26,10 +27,13 @@ - + + + - + diff --git a/ca_gazebo/launch/ekuthon_coop_robots.launch b/ca_gazebo/launch/ekuthon_coop_robots.launch new file mode 100644 index 000000000..813a87b47 --- /dev/null +++ b/ca_gazebo/launch/ekuthon_coop_robots.launch @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/ca_gazebo/launch/spawn_multirobot.launch b/ca_gazebo/launch/spawn_multirobot.launch index 5e2982059..243197f0a 100644 --- a/ca_gazebo/launch/spawn_multirobot.launch +++ b/ca_gazebo/launch/spawn_multirobot.launch @@ -3,7 +3,7 @@ - + @@ -30,5 +30,6 @@ + - \ No newline at end of file + diff --git a/ca_gazebo/src/model_pose_publisher_plugin.cpp b/ca_gazebo/src/model_pose_publisher_plugin.cpp new file mode 100644 index 000000000..31660b251 --- /dev/null +++ b/ca_gazebo/src/model_pose_publisher_plugin.cpp @@ -0,0 +1,80 @@ +#include +#include +#include +#include +#include "gazebo/gazebo.hh" +#include "gazebo/common/Plugin.hh" +#include "gazebo/msgs/msgs.hh" +#include "gazebo/physics/physics.hh" +#include "gazebo/transport/transport.hh" +#include "std_msgs/String.h" +#include "geometry_msgs/Pose.h" + +static const ros::Duration update_rate = ros::Duration(1); // 1 Hz +namespace gazebo +{ +class ModelPosePublisherPlugin : public ModelPlugin +{ + + public: void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/) + { + // Make sure the ROS node for Gazebo has already been initialized + if (!ros::isInitialized()) + { + ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. " + << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); + return; + } + ROS_INFO("Model pose publisher started!"); + ROS_INFO("model Name = %s", _parent->GetName().c_str()); + + // Store the pointer to the model + this->model = _parent; + + // Listen to the update event. This event is broadcast every + // simulation iteration. + this->updateConnection = event::Events::ConnectWorldUpdateBegin( + std::bind(&ModelPosePublisherPlugin::OnUpdate, this)); + this->prev_update_time_ = ros::Time::now(); + + this->rosnode_.reset(new ros::NodeHandle("ModelPose")); + this->pub_ = this->rosnode_->advertise("/ca_gazebo/model_pose", 100); + } + + // Called by the world update start event + public: void OnUpdate() + { + if ((ros::Time::now() - this->prev_update_time_) < update_rate) { + return; + } + + geometry_msgs::Pose msg; + ignition::math::Pose3d pose = this->model->GetLink("link")->WorldPose(); + msg.position.x = pose.Pos().X(); + msg.position.y = pose.Pos().Y(); + msg.position.z = pose.Pos().Z(); + + msg.orientation.x = pose.Rot().X(); + msg.orientation.y = pose.Rot().Y(); + msg.orientation.z = pose.Rot().Z(); + msg.orientation.w = pose.Rot().W(); + + this->pub_.publish(msg); + + this->prev_update_time_ = ros::Time::now(); + } + + private: std::shared_ptr rosnode_; + private: ros::Publisher pub_; + // Pointer to the model + private: physics::ModelPtr model; + + private: ros::Time prev_update_time_; + + // Pointer to the update event connection + private: event::ConnectionPtr updateConnection; + }; + + // Register this plugin with the simulator + GZ_REGISTER_MODEL_PLUGIN(ModelPosePublisherPlugin) +} diff --git a/ca_gazebo/worlds/two_rooms.world b/ca_gazebo/worlds/two_rooms.world new file mode 100644 index 000000000..2754c77ff --- /dev/null +++ b/ca_gazebo/worlds/two_rooms.world @@ -0,0 +1,1002 @@ + + + + 0.001 + 1 + 1000 + 0 0 -9.8 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + EARTH_WGS84 + 0 + 0 + 0 + 0 + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + 1 + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + + + + + 10 + + + 0 + + + 0 0 1 + 100 100 + + + + + + + + 0 + 0 + + 0 + 0 + 1 + + + + 0 + + + 1 + 0 0 0 0 -0 0 + + 0.002741 -0.017964 1.2465 0 -0 0 + 0 + 0 + 0 + + 0 0 0 0 -0 0 + + + 0.9 0.078144 2.51204 + + + + 1 + + + 0 0 0 1 + + 0 + 1 + + + 0 + 10 + 0 0 0 0 -0 0 + + + 0.9 0.078144 2.51204 + + + + + + 0.2 + 0.2 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + + + + 1 + + -1.425 1.425 -0 0 -0 1.5708 + + 2250 + 0 0 0 0 -0 0 + + 1176.09 + 0 + 0 + 2859.38 + 0 + 1691.72 + + + 0 + 0 + 0 + 1 + + 0 0 1.25 0 -0 0 + + + 3 0.15 2.5 + + + + + 1 1 1 1 + + __default__ + + 0 0 0 1 + + + 0 + + 0 + 1 + + + 0 + 10 + 0 0 1.25 0 -0 0 + + + 3 0.15 2.5 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + + 0 2.85 -0 0 -0 0 + + 2250 + 0 0 0 0 -0 0 + + 1176.09 + 0 + 0 + 2859.38 + 0 + 1691.72 + + + 0 + 0 + 0 + 1 + + 0 0 1.25 0 -0 0 + + + 3 0.15 2.5 + + + + + 1 1 1 1 + + __default__ + + 0 0 0 1 + + + 0 + + 0 + 1 + + + 0 + 10 + 0 0 1.25 0 -0 0 + + + 3 0.15 2.5 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + + 1.425 1.425 -0 0 0 -1.5708 + + 2250 + 0 0 0 0 -0 0 + + 1176.09 + 0 + 0 + 2859.38 + 0 + 1691.72 + + + 0 + 0 + 0 + 1 + + 0 0 1.25 0 -0 0 + + + 3 0.15 2.5 + + + + + 1 1 1 1 + + __default__ + + 0 0 0 1 + + + 0 + + 0 + 1 + + + 0 + 10 + 0 0 1.25 0 -0 0 + + + 3 0.15 2.5 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + + 1.425 -1.425 0 0 0 -1.5708 + + 2250 + 0 0 0 0 -0 0 + + 1176.09 + 0 + 0 + 2859.38 + 0 + 1691.72 + + + 0 + 0 + 0 + 1 + + 0 0 1.25 0 -0 0 + + + 3 0.15 2.5 + + + + + 1 1 1 1 + + __default__ + + 0 0 0 1 + + + 0 + + 0 + 1 + + + 0 + 10 + 0 0 1.25 0 -0 0 + + + 3 0.15 2.5 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + + 0 -2.85 0 0 -0 3.14159 + + 2250 + 0 0 0 0 -0 0 + + 1176.09 + 0 + 0 + 2859.38 + 0 + 1691.72 + + + 0 + 0 + 0 + 1 + + 0 0 1.25 0 -0 0 + + + 3 0.15 2.5 + + + + + 1 1 1 1 + + __default__ + + 0 0 0 1 + + + 0 + + 0 + 1 + + + 0 + 10 + 0 0 1.25 0 -0 0 + + + 3 0.15 2.5 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + + -1.425 -1.425 0 0 -0 1.5708 + + 2250 + 0 0 0 0 -0 0 + + 1176.09 + 0 + 0 + 2859.38 + 0 + 1691.72 + + + 0 + 0 + 0 + 1 + + 0 0 1.25 0 -0 0 + + + 3 0.15 2.5 + + + + + 1 1 1 1 + + __default__ + + 0 0 0 1 + 0.7 0.7 0.7 1 + 0.01 0.01 0.01 1 + + + 0 + + 0 + 1 + + + 0 + 10 + 0 0 1.25 0 -0 0 + + + 3 0.15 2.5 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + + -1 0 0 0 -0 0 + + 750 + 0 0 0 0 -0 0 + + 392.031 + 0 + 0 + 453.125 + 0 + 63.9062 + + + 0 + 0 + 0 + 1 + + 0 0 1.25 0 -0 0 + + + 1 0.15 2.5 + + + + + 1 1 1 1 + + __default__ + + 0 0 0 1 + + + 0 + + 0 + 1 + + + 0 + 10 + 0 0 1.25 0 -0 0 + + + 1 0.15 2.5 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + + 1 -0 0 0 -0 3.14159 + + 750 + 0 0 0 0 -0 0 + + 392.031 + 0 + 0 + 453.125 + 0 + 63.9062 + + + 0 + 0 + 0 + 1 + + 0 0 1.25 0 -0 0 + + + 1 0.15 2.5 + + + + + 1 1 1 1 + + __default__ + + 0.7 0.7 0.7 1 + 0.01 0.01 0.01 1 + 0 0 0 1 + + + 0 + + 0 + 1 + + + 0 + 10 + 0 0 1.25 0 -0 0 + + + 1 0.15 2.5 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 1 + + + ground_plane::link + door::link + -0.5 0 0 0 -0 0 + + 0 0 1 + true + + -3.14 + 3.14 + + + 0 + 0 + 0 + 0 + + + + + + 0 + 0.2 + + + 0 + 0.2 + + + + + + + + 0.59881 -4.06701 27.182 -2.2526e-16 1.3218 2.2322 + orbit + + + +