-
Notifications
You must be signed in to change notification settings - Fork 132
/
blocks_spawner.cpp
174 lines (147 loc) · 6.48 KB
/
blocks_spawner.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
// spawn the red blocks on the conveyor belt
// and give them initial speed (by apply_body_wrench) to slide on conveyor
//ros communications:
// spawn model throught gazebo service: /gazebo/spawn_urdf_model
// initialize blocks speed: /gazebo/apply_body_wrench
// get urdf file path of blocks from parameter servicer
//publish all current blocks through topic: /current_blocks
#include <ros/ros.h>
#include <iostream>
#include <sstream>
#include <fstream>
#include <string>
#include <urdf/model.h>
#include <gazebo_msgs/SpawnModel.h>
#include <gazebo_msgs/ApplyBodyWrench.h>
#include <std_msgs/Int8MultiArray.h>
#include <gazebo_msgs/SetModelState.h>
//int to string converter
std::string intToString (int a) {
std::stringstream ss;
ss << a;
return ss.str();
}
int main(int argc, char **argv) {
ros::init(argc, argv, "blocks_spawner");
ros::NodeHandle nh;
srand(time(0));
//service client for service /gazebo/spawn_urdf_model
ros::ServiceClient spawnClient = nh.serviceClient<gazebo_msgs::SpawnModel>("/gazebo/spawn_urdf_model");
gazebo_msgs::SpawnModel::Request spawn_model_req;
gazebo_msgs::SpawnModel::Response spawn_model_resp;
ros::ServiceClient wrenchClient = nh.serviceClient<gazebo_msgs::ApplyBodyWrench>("/gazebo/apply_body_wrench");
gazebo_msgs::ApplyBodyWrench::Request apply_wrench_req;
gazebo_msgs::ApplyBodyWrench::Response apply_wrench_resp;
ros::ServiceClient setstateClient = nh.serviceClient<gazebo_msgs::SetModelState>("/gazebo/set_model_state");
gazebo_msgs::ApplyBodyWrench::Request set_state_req;
gazebo_msgs::ApplyBodyWrench::Response set_state_resp;
//publisher for current_blocks
ros::Publisher current_blocks_publisher = nh.advertise<std_msgs::Int8MultiArray>("current_blocks",1);
std_msgs::Int8MultiArray current_blocks_msg;
current_blocks_msg.data.clear();
// make sure /gazebo/spawn_urdf_model service is service_ready
bool service_ready = false;
while (!service_ready){
service_ready = ros::service::exists("/gazebo/spawn_urdf_model", true);
ROS_INFO("waiting for spawn_urdf_model service");
ros::Duration(0.5).sleep();
}
ROS_INFO("spawn_urdf_model service is ready");
service_ready = false;
while (!service_ready) {
service_ready = ros::service::exists("/gazebo/apply_body_wrench", true);
ROS_INFO("waiting for apply_body_wrench service");
ros::Duration(0.5).sleep();
}
ROS_INFO("apply_body_wrench service is ready");
service_ready = false;
while (!service_ready) {
service_ready = ros::service::exists("/gazebo/set_model_state", true);
ROS_INFO("waiting for set_model_state service");
ros::Duration(0.5).sleep();
}
ROS_INFO("set_model_state service is ready");
//get file path of blocks from parameter service
std::string red_box_path;
bool get_red_path;
get_red_path = nh.getParam("/red_box_path", red_box_path);
if (!(get_red_path)){
return 0;}
else{ROS_INFO_STREAM(red_box_path << " has been extracted");
}
std::ifstream red_inXml(red_box_path.c_str());
std::stringstream red_strStream;
std::string red_xmlStr;
/*red_inXml.open(red_box_path.c_str());*/
red_strStream << red_inXml.rdbuf();
red_xmlStr = red_strStream.str();
// ROS_INFO_STREAM("urdf: \n" <<red_xmlStr);
// prepare the pawn model service message
spawn_model_req.initial_pose.position.x = 2;
spawn_model_req.initial_pose.position.z = 0.2;
spawn_model_req.initial_pose.orientation.x=0.0;
spawn_model_req.initial_pose.orientation.y=0.0;
spawn_model_req.initial_pose.orientation.z=0.0;
spawn_model_req.initial_pose.orientation.w=1.0;
spawn_model_req.reference_frame = "world";
ros::Time time_temp(0, 0);
ros::Duration duration_temp(0, 1000000);
apply_wrench_req.wrench.force.x = -5.1;
apply_wrench_req.wrench.force.y = 0.0;
apply_wrench_req.wrench.force.z = 0.0;
apply_wrench_req.start_time = time_temp;
apply_wrench_req.duration = duration_temp;
apply_wrench_req.reference_frame = "world";
int i =0;
while (ros::ok()){
std::string index = intToString(i);
std::string model_name;
spawn_model_req.initial_pose.position.y = (float)rand()/(float)(RAND_MAX) * 0.4; // random between -0.4 to 0.4
ROS_INFO_STREAM("y position of new box: "
<< spawn_model_req.initial_pose.position.y);
model_name = "red_blocks_" + index; // initialize model_name
spawn_model_req.model_name = model_name;
spawn_model_req.robot_namespace = model_name;
spawn_model_req.model_xml = red_xmlStr;
bool call_service = spawnClient.call(spawn_model_req, spawn_model_resp);
if (call_service) {
if (spawn_model_resp.success) {
ROS_INFO_STREAM(model_name << " has been spawned");
}
else {
ROS_INFO_STREAM(model_name << " spawn failed");
}
}
else {
ROS_INFO("fail in first call");
ROS_ERROR("fail to connect with gazebo server");
return 0;
}
// prepare apply body wrench service message
apply_wrench_req.body_name = model_name + "::base_link";
// call apply body wrench service
call_service = wrenchClient.call(apply_wrench_req, apply_wrench_resp);
if (call_service) {
if (apply_wrench_resp.success) {
ROS_INFO_STREAM(model_name << " speed initialized");
}
else {
ROS_INFO_STREAM(model_name << " fail to initialize speed");
}
}
else {
ROS_ERROR("fail to connect with gazebo server");
return 0;
}
// publish current cylinder blocks status, all cylinder blocks will be published
// no matter if it's successfully spawned, or successfully initialized in speed
current_blocks_publisher.publish(current_blocks_msg);
// loop end, increase index by 1, add blank line
i = i + 1;
ROS_INFO_STREAM("");
ros::spinOnce();
ros::Duration(20.0).sleep(); // frequency control, spawn one cylinder in each loop
// delay time decides density of the cylinders
}
return 0;
}