Skip to content

Commit 04ba71d

Browse files
SS47816SS47816
committed
feat: add object_spwner_gz_plugin
1 parent 5eea193 commit 04ba71d

File tree

4 files changed

+150
-1
lines changed

4 files changed

+150
-1
lines changed

‎src/me5413_world/CMakeLists.txt‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@ find_package(catkin REQUIRED COMPONENTS
1919
tf2_eigen
2020
tf2_geometry_msgs
2121
)
22+
find_package(gazebo REQUIRED)
2223

2324
catkin_package(
2425
INCLUDE_DIRS include
@@ -30,10 +31,17 @@ catkin_package(
3031
include_directories(
3132
include
3233
${catkin_INCLUDE_DIRS}
34+
${GAZEBO_INCLUDE_DIRS}
3335
)
3436

37+
link_directories(${GAZEBO_LIBRARY_DIRS})
38+
list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}")
39+
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
40+
3541
# Add Nodes
3642
add_executable(goal_publisher_node src/goal_publisher_node.cpp)
3743
target_link_libraries(goal_publisher_node ${catkin_LIBRARIES})
3844
add_dependencies(goal_publisher_node ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS})
3945

46+
add_library(object_spawner_gz_plugin SHARED src/object_spawner_gz_plugin.cpp)
47+
target_link_libraries(object_spawner_gz_plugin ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})

‎src/me5413_world/package.xml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -65,6 +65,6 @@
6565
<!-- The export tag contains other, unspecified, tags -->
6666
<export>
6767
<!-- Other tools can request additional information be placed here -->
68-
68+
<gazebo plugin_path="${prefix}/lib" gazebo_media_path="${prefix}" />
6969
</export>
7070
</package>

‎src/me5413_world/src/object_spawner_gz_plugin.cpp‎

Lines changed: 139 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,139 @@
1+
#include <ignition/math/Pose3.hh>
2+
#include "gazebo/physics/physics.hh"
3+
#include "gazebo/common/common.hh"
4+
#include "gazebo/gazebo.hh"
5+
#include <cstdlib>
6+
#include <ctime>
7+
8+
namespace gazebo
9+
{
10+
class ObjectSpawner : public WorldPlugin
11+
{
12+
public:
13+
void Load(physics::WorldPtr _parent, sdf::ElementPtr /*_sdf*/)
14+
{
15+
// Option 1: Insert model from file via function call.
16+
// The filename must be in the GAZEBO_MODEL_PATH environment variable.
17+
//_parent->InsertModelFile("model://box");
18+
19+
// Option 2: Insert model from string via function call.
20+
// Insert a sphere model from string
21+
22+
// sdf::SDF sphereSDF;
23+
// sphereSDF.SetFromString(
24+
// "<sdf version ='1.4'>\
25+
// <model name ='sphere'>\
26+
// <pose>10 0 0 0 0 0</pose>\
27+
// <link name ='link'>\
28+
// <pose>0 0 .5 0 0 0</pose>\
29+
// <collision name ='collision'>\
30+
// <geometry>\
31+
// <sphere><radius>0.5</radius></sphere>\
32+
// </geometry>\
33+
// </collision>\
34+
// <visual name ='visual'>\
35+
// <geometry>\
36+
// <sphere><radius>0.5</radius></sphere>\
37+
// </geometry>\
38+
// </visual>\
39+
// </link>\
40+
// </model>\
41+
// </sdf>");
42+
// // Demonstrate using a custom model name.
43+
// sdf::ElementPtr model = sphereSDF.Root()->GetElement("model");
44+
// model->GetAttribute("name")->SetFromString("unique_sphere");
45+
// _parent->InsertModelSDF(sphereSDF);
46+
47+
// Option 3: Insert model from file via message passing.
48+
{
49+
// Create a new transport node
50+
transport::NodePtr node(new transport::Node());
51+
52+
// Initialize the node with the world name
53+
node->Init(_parent->Name());
54+
55+
// Create a publisher on the ~/factory topic
56+
transport::PublisherPtr factoryPub = node->Advertise<msgs::Factory>("~/factory");
57+
58+
// Create the message
59+
msgs::Factory msg_1;
60+
msgs::Factory msg_2;
61+
msgs::Factory msg_3;
62+
msgs::Factory msg_cone;
63+
64+
// Generate random points within a 10 by 8 area with a distance greater than 4.3
65+
ignition::math::Vector2d p1, p2, p3;
66+
67+
do
68+
{
69+
p1 = ignition::math::Vector2d(
70+
static_cast<double>(std::rand()) / RAND_MAX * 8.0 + 8,
71+
1 - static_cast<double>(std::rand()) / RAND_MAX * 7.25);
72+
73+
p2 = ignition::math::Vector2d(
74+
static_cast<double>(std::rand()) / RAND_MAX * 8.0 + 8,
75+
-6.25 + static_cast<double>(std::rand()) / RAND_MAX * 7.25);
76+
77+
p3 = ignition::math::Vector2d(
78+
static_cast<double>(std::rand()) / RAND_MAX * 8.0 + 8,
79+
-6.25 + static_cast<double>(std::rand()) / RAND_MAX * 7.25);
80+
} while ((p1 - p2).Length() <= 4.3 ||
81+
(p1 - p3).Length() <= 4.3 ||
82+
(p2 - p3).Length() <= 4.3 ||
83+
(p1.X() > 16.2 && p1.X() < 16.9 && p1.Y() > -6.16 && p1.Y() < -5.76) ||
84+
(p2.X() > 16.2 && p2.X() < 16.9 && p2.Y() > -6.16 && p2.Y() < -5.76) ||
85+
(p3.X() > 16.2 && p3.X() < 16.9 && p3.Y() > -6.16 && p3.Y() < -5.76));
86+
87+
// Model file to load
88+
msg_1.set_sdf_filename("model://number1");
89+
msg_2.set_sdf_filename("model://number2");
90+
msg_3.set_sdf_filename("model://number3");
91+
msg_cone.set_sdf_filename("model://construction_cone");
92+
93+
// Seed for random number generation
94+
std::srand(std::time(0));
95+
96+
if (std::rand() % 2 == 0)
97+
{
98+
msgs::Set(msg_cone.mutable_pose(),
99+
ignition::math::Pose3d(
100+
ignition::math::Vector3d(12.7, 2.5, 0.4),
101+
ignition::math::Quaterniond(0, 0, 0)));
102+
}
103+
else
104+
{
105+
msgs::Set(msg_cone.mutable_pose(),
106+
ignition::math::Pose3d(
107+
ignition::math::Vector3d(16.9, 2.5, 0.4),
108+
ignition::math::Quaterniond(0, 0, 0)));
109+
}
110+
111+
// Pose to initialize the model to
112+
msgs::Set(msg_1.mutable_pose(),
113+
ignition::math::Pose3d(
114+
ignition::math::Vector3d(p1.X(), p1.Y(), 0.4),
115+
ignition::math::Quaterniond(0, 0, 0)));
116+
117+
msgs::Set(msg_2.mutable_pose(),
118+
ignition::math::Pose3d(
119+
ignition::math::Vector3d(p2.X(), p2.Y(), 0.4),
120+
ignition::math::Quaterniond(0, 0, 0)));
121+
122+
msgs::Set(msg_3.mutable_pose(),
123+
ignition::math::Pose3d(
124+
ignition::math::Vector3d(p3.X(), p3.Y(), 0.4),
125+
ignition::math::Quaterniond(0, 0, 0)));
126+
127+
// Send the message
128+
factoryPub->Publish(msg_1);
129+
factoryPub->Publish(msg_2);
130+
factoryPub->Publish(msg_3);
131+
factoryPub->Publish(msg_cone);
132+
}
133+
}
134+
};
135+
136+
// Register this plugin with the simulator
137+
GZ_REGISTER_WORLD_PLUGIN(ObjectSpawner)
138+
139+
} // namespace gazebo

‎src/me5413_world/worlds/me5413_project.world‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -5497,5 +5497,7 @@
54975497
</link>
54985498
<pose frame=''>-0.759737 -4.1022 -0 1.5708 -0 1.5708</pose>
54995499
</model>
5500+
5501+
<plugin name="object_spawner_gz_plugin" filename="libobject_spawner_gz_plugin.so"/>
55005502
</world>
55015503
</sdf>

0 commit comments

Comments
 (0)