Newer
Older
#include <cmath>
#include <cstdint>
#include <cstring>
#include <gz/msgs/empty.pb.h>
#include <gz_gep_tools/control_over_gz.hh>
#include <gz/msgs.hh>
#include <gz/transport.hh>
#include <gz/sim/EntityComponentManager.hh>
#include <gz/sim/components/Joint.hh>
#include <gz/sim/Joint.hh>
ControlOverGz::ControlOverGz(std::string &world_prefix, bool debug_level)
: world_prefix_(world_prefix), debug_level_(debug_level)
{
/// Create control_gzsim topic name
control_service_gzsim_ = world_prefix+std::string("/control");
/// State gzsim service
state_service_gzsim_ = world_prefix+std::string("/state");
std::string gazebo_clock_topic_name;
gazebo_clock_topic_name = world_prefix + std::string("/clock");
node_.Subscribe<ControlOverGz,gz::msgs::Clock>(gazebo_clock_topic_name,
&ControlOverGz::CallbackClock,
this);
gz_sim_time_ = std::nan("1");
wrld_ctrl_state_srv_gzsim_ = control_service_gzsim_ + std::string("/state");
}
void ControlOverGz::CallbackClock(const gz::msgs::Clock &a_gz_time_msg) {
if (a_gz_time_msg.has_sim())
{
gz_sim_time_ = (double)a_gz_time_msg.sim().sec() +
1e-9*(double)a_gz_time_msg.sim().nsec();
if (debug_level_)
std::cerr << "ControlOverGz::CallbackClock: Time: " << a_gz_time_msg.sim().sec()
<< " " << a_gz_time_msg.sim().nsec()
<< std::endl;
}
}
double ControlOverGz::GetSimTime() {
return gz_sim_time_;
}
bool ControlOverGz::Step()
{
gz::msgs::WorldControl req_world_ctrl;
gz::msgs::Boolean rep_bool;
unsigned int timeout = 3000;
/// Set reset for everything
req_world_ctrl.set_multi_step(1);
req_world_ctrl.set_pause(true);
/// Do request.
bool result=true;
if (!node_.Request( control_service_gzsim_, req_world_ctrl, timeout, rep_bool, result)) {
std::cerr << "Unable to send reset request !"
<< std::endl;
result =false;
}
///set_allocated_reset(&req_world_reset);
return result;
}
bool ControlOverGz::Reset()
{
gz::msgs::WorldControl req_world_ctrl;
gz::msgs::WorldReset * req_world_reset;
unsigned int timeout = 3000;
req_world_reset = new gz::msgs::WorldReset();
req_world_reset->set_all(true);
req_world_ctrl.set_pause(true);
/// Set the world reset inside the world control reset.
req_world_ctrl.set_allocated_reset(req_world_reset);
if (!node_.Request( control_service_gzsim_, req_world_ctrl, timeout, rep_bool, result)) {
std::cerr << "Unable to send reset request !"
<< std::endl;
result =false;
}
/// World Reset will be released because out of scope.
return result;
}
bool ControlOverGz::SendPauseRequest(bool abool)
{
gz::msgs::WorldControl req_world_ctrl;
gz::msgs::Boolean rep_bool;
unsigned int timeout = 5000;
/// Set the world reset inside the world control reset.
req_world_ctrl.set_pause(abool);
node_.Request( control_service_gzsim_, req_world_ctrl, timeout, rep_bool, result);
return result;
}
bool ControlOverGz::Pause() { return SendPauseRequest(true); }
bool ControlOverGz::Start() { return SendPauseRequest(false); }
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
bool ControlOverGz::ReadWorldStateToInitECM()
{
/// Gazebo messages
/// Response as Serialized map of the entities
gz::msgs::SerializedStepMap rep_serialized_step_map;
/// Request does not have request argument.
gz::msgs::Empty req_empty_msg;
unsigned int timeout = 3000;
gz::msgs::Boolean rep_bool;
bool result=true;
/// Get World Control State.
/// by calling the /prefix_world/state Gazebo service.
if (!node_.Request(state_service_gzsim_,
req_empty_msg,
timeout,
rep_serialized_step_map,
result)) {
std::cerr << "Unable to send reset request ! timeout "
<< state_service_gzsim_ << " "
<< result
<< std::endl;
result =false;
}
/// Initialization of the robot's Entity Component Manager.
Robot_ECM_.SetState(*rep_serialized_step_map.mutable_state());
return result;
}
void ControlOverGz::DisplayLinkValues()
{
auto entities = Robot_ECM_.EntitiesByComponents( gz::sim::components::Link());
for (auto it_entity = entities.begin();
it_entity != entities.end();
it_entity++)
{
gz::sim::Link a_link(*it_entity);
std::string default_link_name("noname");
std::cout << "Link name:" << a_link.Name(Robot_ECM_).value_or(default_link_name);
std::vector<gz::sim::Entity> a_link_collisions = a_link.Collisions(Robot_ECM_);
std::cout << "Collisions:" << a_link_collisions.size() << std::endl;
}
}
void ControlOverGz::DisplayJointValues()
{
auto entities = Robot_ECM_.EntitiesByComponents( gz::sim::components::Joint());
for (auto it_Entity = entities.begin();
it_Entity != entities.end();
it_Entity++)
{
gz::sim::Joint aJoint(*it_Entity);
// std::cout << "Joint name:" << aJoint.Name(Robot_ECM_).value_or("No joint name");
std::vector<double> empty_vecd;
std::vector<double> pos_vecd = aJoint.Position(Robot_ECM_).value_or(empty_vecd);
if (pos_vecd.size()!=0)
std::cout << " " << "Position: " << pos_vecd[0];
else
std::cout << " no pos ";
std::vector<double> vel_vecd = aJoint.Velocity(Robot_ECM_).value_or(empty_vecd);
if (vel_vecd.size()!=0)
std::cout << " " << "Velocity: " << vel_vecd[1];
else
std::cout << " no vel ";
std::cout << std::endl;
}
}
bool ControlOverGz::SendWorldControlStateToInitECM(const RobotCtrlJointInfos &rbt_ctrl_joint_infos)
{
gz::msgs::WorldControlState req_world_ctrl_state;
gz::msgs::SerializedState * req_serialized_state;
unsigned int timeout = 3000;
gz::msgs::Boolean rep_bool;
bool result=true;
// Modify joint entities in Robot_ECM.
auto entities = Robot_ECM_.EntitiesByComponents( gz::sim::components::Joint());
for (auto it_Entity = entities.begin();
it_Entity != entities.end();
it_Entity++)
{
gz::sim::Joint aJoint(*it_Entity);
std::string jointName = aJoint.Name(Robot_ECM_).value_or("No joint name");
// std::cout << "Joint name:" << jointName;
std::vector<double> one_vecd;
auto a_ctrl_joint_info = rbt_ctrl_joint_infos.find(jointName);
if (a_ctrl_joint_info != rbt_ctrl_joint_infos.end())
{
one_vecd.push_back(a_ctrl_joint_info->second.pos_des);
// std::cout << " " << one_vecd[0] << std::endl;
aJoint.ResetPosition(Robot_ECM_,one_vecd);
}
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
}
/// Create the serialized state.
req_serialized_state = new gz::msgs::SerializedState();
/// Generate the serialized state msg from the ECM.
*req_serialized_state = Robot_ECM_.State();
/// Link it in the WorldControlState msg
req_world_ctrl_state.set_allocated_state(req_serialized_state);
/// Set World Control State.
/// by calling the /prefix_world/state/control Gazebo service.
if (!node_.Request(wrld_ctrl_state_srv_gzsim_,
req_world_ctrl_state,
timeout,
rep_bool,
result)) {
std::cerr << "Unable to send reset request ! timeout "
<< wrld_ctrl_state_srv_gzsim_ << " "
<< result
<< std::endl;
result =false;
}
return true;
}