From 6f0453885cef9a80f1792030d2232506234895e3 Mon Sep 17 00:00:00 2001 From: jlurgo Date: Fri, 18 Oct 2019 17:47:36 -0300 Subject: [PATCH 1/7] 2 rooms with separation door --- ca_gazebo/launch/ekuthon_coop_robots.launch | 12 + ca_gazebo/worlds/two_rooms.world | 1132 +++++++++++++++++++ 2 files changed, 1144 insertions(+) create mode 100644 ca_gazebo/launch/ekuthon_coop_robots.launch create mode 100644 ca_gazebo/worlds/two_rooms.world diff --git a/ca_gazebo/launch/ekuthon_coop_robots.launch b/ca_gazebo/launch/ekuthon_coop_robots.launch new file mode 100644 index 00000000..477f828c --- /dev/null +++ b/ca_gazebo/launch/ekuthon_coop_robots.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/ca_gazebo/worlds/two_rooms.world b/ca_gazebo/worlds/two_rooms.world new file mode 100644 index 00000000..253fbee4 --- /dev/null +++ b/ca_gazebo/worlds/two_rooms.world @@ -0,0 +1,1132 @@ + + + + 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.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.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 + + + + + + + + 0.05 + 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.925481 0.078144 2.51204 + + + + 1 + + + 0 0 0 1 + + 0 + 1 + + + 0 + 10 + 0 0 0 0 -0 0 + + + 0.925481 0.078144 2.51204 + + + + + + 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 + + + + + + + Wall_0 + Wall_6 + 0 0 0 0 -0 0 + + + + 0 + 0.2 + + + 0 + 0.2 + + + + + + Wall_1 + Wall_0 + 0 0 0 0 -0 0 + + + + 0 + 0.2 + + + 0 + 0.2 + + + + + + Wall_3 + Wall_2 + 0 0 0 0 -0 0 + + + + 0 + 0.2 + + + 0 + 0.2 + + + + + + Wall_4 + Wall_3 + 0 0 0 0 -0 0 + + 1 0 0 + 0 + + -1.79769e+308 + 1.79769e+308 + -1 + -1 + + + 0 + 0 + 0 + + + + + + 0 + 0.2 + + + 0 + 0.2 + + + + + + Wall_4 + Wall_5 + 0 0 0 0 -0 0 + + + + 0 + 0.2 + + + 0 + 0.2 + + + + + + Wall_5 + Wall_0 + 0 0 0 0 -0 0 + + + + 0 + 0.2 + + + 0 + 0.2 + + + + + + Wall_8 + Wall_3 + 0 0 0 0 -0 0 + + + + 0 + 0.2 + + + 0 + 0.2 + + + + + + Wall_6 + door + -0.5 0 0 0 -0 0 + + 0 0 1 + 0 + + -3.14 + 3.14 + -1 + -1 + + + 0 + 0 + 0 + 0 + + + + + + 0 + 0.2 + + + 0 + 0.2 + + + + + 0 + 1 + + + + 0.59881 -4.06701 27.182 -2.2526e-16 1.3218 2.2322 + orbit + + + + From 3c30e23e5d9364e1baa1f48579fba20bf62351d0 Mon Sep 17 00:00:00 2001 From: jlurgo Date: Mon, 21 Oct 2019 18:52:25 -0300 Subject: [PATCH 2/7] added pose and spawned a second robot --- ca_gazebo/launch/ekuthon_coop_robots.launch | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/ca_gazebo/launch/ekuthon_coop_robots.launch b/ca_gazebo/launch/ekuthon_coop_robots.launch index 477f828c..aca74d68 100644 --- a/ca_gazebo/launch/ekuthon_coop_robots.launch +++ b/ca_gazebo/launch/ekuthon_coop_robots.launch @@ -4,9 +4,17 @@ + - + - + + + + + + + + From d076aab95200fba4f53a4c130dd62a445084cefb Mon Sep 17 00:00:00 2001 From: jlurgo Date: Fri, 1 Nov 2019 12:06:53 -0300 Subject: [PATCH 3/7] Model plugin and door hinge working --- ca_gazebo/CMakeLists.txt | 6 +- ca_gazebo/src/model_pose_publisher_plugin.cpp | 70 + ca_gazebo/worlds/two_rooms.world | 1986 ++++++++--------- 3 files changed, 1003 insertions(+), 1059 deletions(-) create mode 100644 ca_gazebo/src/model_pose_publisher_plugin.cpp diff --git a/ca_gazebo/CMakeLists.txt b/ca_gazebo/CMakeLists.txt index 7d909fa7..c4b4a8a3 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/src/model_pose_publisher_plugin.cpp b/ca_gazebo/src/model_pose_publisher_plugin.cpp new file mode 100644 index 00000000..ed8b396b --- /dev/null +++ b/ca_gazebo/src/model_pose_publisher_plugin.cpp @@ -0,0 +1,70 @@ +#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" + +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("/model_pose_plugin")); + // this->pose_pub_ = this->rosnode_->advertise("model_pose", 1000); + } + // Called by the world update start event + public: void OnUpdate() + { + if ((ros::Time::now() - this->prev_update_time_) < update_rate) { + return; + } + ROS_INFO("publishing door pose"); + ROS_INFO("door_pose x=%f y=%f z=%f", + this->model->RelativePose().Pos().X(), + this->model->RelativePose().Pos().Y(), + this->model->RelativePose().Pos().Z()); + + this->prev_update_time_ = ros::Time::now(); + // Apply a small linear velocity to the model. + //this->model->SetLinearVel(ignition::math::Vector3d(0, .3, 0)); + } + + // 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 index 253fbee4..5615c7fc 100644 --- a/ca_gazebo/worlds/two_rooms.world +++ b/ca_gazebo/worlds/two_rooms.world @@ -1,66 +1,5 @@ - - 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.001 1 @@ -79,1025 +18,958 @@ 0 0 - - - -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 + + 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 + + + + - 0 + 100 + 50 - - - - 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 + + + + + + + 10 + + + 0 + + + 0 0 1 + 100 100 + + + + + + + + 0 + 0 + + 0 + 0 + 1 + + + + 0 + + + 0.05 + 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 + 1 + 1 + 0 0 0 + 0 + 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 + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 - 0 + 0 + 0.2 + 1e+13 + 1 + 0.01 + 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 + + 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 + + + + - 0 + 1 + 1 + 0 0 0 + 0 + 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 + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 - 0 + 0 + 0.2 + 1e+13 + 1 + 0.01 + 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 + + 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 + + + + - 0 + 1 + 1 + 0 0 0 + 0 + 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 + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 - 0 + 0 + 0.2 + 1e+13 + 1 + 0.01 + 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 + + 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 + + + + - 0 + 1 + 1 + 0 0 0 + 0 + 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.05 - 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.925481 0.078144 2.51204 - - - - 1 - - - 0 0 0 1 - - 0 - 1 - - - 0 - 10 - 0 0 0 0 -0 0 - - - 0.925481 0.078144 2.51204 - - - - - - 1 - 1 - 0 0 0 - 0 - 0 - - - 1 - 0 - 0 - 1 + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 - 0 + 0 + 0.2 + 1e+13 + 1 + 0.01 + 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 - - - - - - - Wall_0 - Wall_6 - 0 0 0 0 -0 0 - - - - 0 - 0.2 - - - 0 - 0.2 - - - - - - Wall_1 - Wall_0 - 0 0 0 0 -0 0 - - - - 0 - 0.2 - - - 0 - 0.2 - - - - - - Wall_3 - Wall_2 - 0 0 0 0 -0 0 - - - - 0 - 0.2 - - - 0 - 0.2 - - - - - - Wall_4 - Wall_3 - 0 0 0 0 -0 0 - - 1 0 0 - 0 - - -1.79769e+308 - 1.79769e+308 - -1 - -1 - - - 0 - 0 - 0 - - - - - - 0 - 0.2 - - - 0 - 0.2 - - - - - - Wall_4 - Wall_5 - 0 0 0 0 -0 0 - - - - 0 - 0.2 - - - 0 - 0.2 - - - - - - Wall_5 - Wall_0 - 0 0 0 0 -0 0 - - - - 0 - 0.2 - - - 0 - 0.2 - - - - - - Wall_8 - Wall_3 - 0 0 0 0 -0 0 - - - - 0 - 0.2 - - - 0 - 0.2 - - - - - - Wall_6 - door + + 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 - 0 + true -3.14 3.14 - -1 - -1 0 @@ -1119,8 +991,6 @@ - 0 - 1 From b5b8d491b478269255004c5efe3c0b561e951172 Mon Sep 17 00:00:00 2001 From: jlurgo Date: Wed, 6 Nov 2019 01:15:15 -0300 Subject: [PATCH 4/7] Door pose publication --- ca_gazebo/src/model_pose_publisher_plugin.cpp | 31 ++++++++++++------- 1 file changed, 20 insertions(+), 11 deletions(-) diff --git a/ca_gazebo/src/model_pose_publisher_plugin.cpp b/ca_gazebo/src/model_pose_publisher_plugin.cpp index ed8b396b..5092f5e7 100644 --- a/ca_gazebo/src/model_pose_publisher_plugin.cpp +++ b/ca_gazebo/src/model_pose_publisher_plugin.cpp @@ -8,6 +8,7 @@ #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 @@ -36,26 +37,34 @@ class ModelPosePublisherPlugin : public ModelPlugin std::bind(&ModelPosePublisherPlugin::OnUpdate, this)); this->prev_update_time_ = ros::Time::now(); - // this->rosnode_.reset(new ros::NodeHandle("/model_pose_plugin")); - // this->pose_pub_ = this->rosnode_->advertise("model_pose", 1000); + this->rosnode_ = 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; - } - ROS_INFO("publishing door pose"); - ROS_INFO("door_pose x=%f y=%f z=%f", - this->model->RelativePose().Pos().X(), - this->model->RelativePose().Pos().Y(), - this->model->RelativePose().Pos().Z()); + } + + geometry_msgs::Pose msg; + msg.position.x = this->model->GetLink("link")->WorldPose().Pos().X(); + msg.position.y = this->model->GetLink("link")->WorldPose().Pos().Y(); + msg.position.z = this->model->GetLink("link")->WorldPose().Pos().Z(); - this->prev_update_time_ = ros::Time::now(); - // Apply a small linear velocity to the model. - //this->model->SetLinearVel(ignition::math::Vector3d(0, .3, 0)); + msg.orientation.x = this->model->GetLink("link")->WorldPose().Rot().X(); + msg.orientation.y = this->model->GetLink("link")->WorldPose().Rot().Y(); + msg.orientation.z = this->model->GetLink("link")->WorldPose().Rot().Z(); + msg.orientation.w = this->model->GetLink("link")->WorldPose().Rot().W(); + + this->pub_.publish(msg); + + this->prev_update_time_ = ros::Time::now(); } + private: ros::NodeHandle* rosnode_; + private: ros::Publisher pub_; // Pointer to the model private: physics::ModelPtr model; From 3098a19ccb44da6270646c0f900a041610854318 Mon Sep 17 00:00:00 2001 From: jlurgo Date: Wed, 6 Nov 2019 14:39:54 -0300 Subject: [PATCH 5/7] Changes in launchfile --- ca_gazebo/launch/ekuthon_coop_robots.launch | 11 ++--------- 1 file changed, 2 insertions(+), 9 deletions(-) diff --git a/ca_gazebo/launch/ekuthon_coop_robots.launch b/ca_gazebo/launch/ekuthon_coop_robots.launch index aca74d68..ec0df62c 100644 --- a/ca_gazebo/launch/ekuthon_coop_robots.launch +++ b/ca_gazebo/launch/ekuthon_coop_robots.launch @@ -1,11 +1,4 @@ - - - - - - - @@ -14,7 +7,7 @@ - - + + From 55f1af2eff8b3a270c4af7288d3536084941ec4e Mon Sep 17 00:00:00 2001 From: jlurgo Date: Wed, 6 Nov 2019 15:18:46 -0300 Subject: [PATCH 6/7] More changes in launchfile --- ca_gazebo/launch/create_empty_world.launch | 16 ++++++++++------ ca_gazebo/launch/ekuthon_coop_robots.launch | 12 ++++++------ ca_gazebo/launch/spawn_multirobot.launch | 5 +++-- ca_gazebo/worlds/two_rooms.world | 6 +++--- 4 files changed, 22 insertions(+), 17 deletions(-) diff --git a/ca_gazebo/launch/create_empty_world.launch b/ca_gazebo/launch/create_empty_world.launch index 79bb84d6..e9a05ecf 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 index ec0df62c..813a87b4 100644 --- a/ca_gazebo/launch/ekuthon_coop_robots.launch +++ b/ca_gazebo/launch/ekuthon_coop_robots.launch @@ -1,11 +1,11 @@ - - - + + + - - - + + + diff --git a/ca_gazebo/launch/spawn_multirobot.launch b/ca_gazebo/launch/spawn_multirobot.launch index 5e298205..243197f0 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/worlds/two_rooms.world b/ca_gazebo/worlds/two_rooms.world index 5615c7fc..2754c77f 100644 --- a/ca_gazebo/worlds/two_rooms.world +++ b/ca_gazebo/worlds/two_rooms.world @@ -84,7 +84,7 @@ 0 - 0.05 + 1 0 0 0 0 -0 0 0.002741 -0.017964 1.2465 0 -0 0 @@ -122,8 +122,8 @@ - 1 - 1 + 0.2 + 0.2 0 0 0 0 0 From 0fb59fc2c7ce50d11e1cfeccfdfe7879e6d6bd1b Mon Sep 17 00:00:00 2001 From: jlurgo Date: Wed, 6 Nov 2019 16:01:35 -0300 Subject: [PATCH 7/7] Using smart pinter for rosnode property of plugin --- ca_gazebo/src/model_pose_publisher_plugin.cpp | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/ca_gazebo/src/model_pose_publisher_plugin.cpp b/ca_gazebo/src/model_pose_publisher_plugin.cpp index 5092f5e7..31660b25 100644 --- a/ca_gazebo/src/model_pose_publisher_plugin.cpp +++ b/ca_gazebo/src/model_pose_publisher_plugin.cpp @@ -37,7 +37,7 @@ class ModelPosePublisherPlugin : public ModelPlugin std::bind(&ModelPosePublisherPlugin::OnUpdate, this)); this->prev_update_time_ = ros::Time::now(); - this->rosnode_ = new ros::NodeHandle("ModelPose"); + this->rosnode_.reset(new ros::NodeHandle("ModelPose")); this->pub_ = this->rosnode_->advertise("/ca_gazebo/model_pose", 100); } @@ -49,21 +49,22 @@ class ModelPosePublisherPlugin : public ModelPlugin } geometry_msgs::Pose msg; - msg.position.x = this->model->GetLink("link")->WorldPose().Pos().X(); - msg.position.y = this->model->GetLink("link")->WorldPose().Pos().Y(); - msg.position.z = this->model->GetLink("link")->WorldPose().Pos().Z(); + 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 = this->model->GetLink("link")->WorldPose().Rot().X(); - msg.orientation.y = this->model->GetLink("link")->WorldPose().Rot().Y(); - msg.orientation.z = this->model->GetLink("link")->WorldPose().Rot().Z(); - msg.orientation.w = this->model->GetLink("link")->WorldPose().Rot().W(); + 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: ros::NodeHandle* rosnode_; + private: std::shared_ptr rosnode_; private: ros::Publisher pub_; // Pointer to the model private: physics::ModelPtr model;