Hello I have a code snippet here in C++ which spawns a red box in Gazebo at the position 'y' which is here, the spawn_model_req.initial_pose.position.y variable. I want to create a list of numbers e.g. [-0.2,-0.3,-0.4,0.4,0.5] and choose random number for that variable to randomly spawn the red box at any 'y' position.
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;
P.S: I have attached the full code below
// 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;
}
Aucun commentaire:
Enregistrer un commentaire