Skip to content
Snippets Groups Projects
Commit 2612378e authored by Olivier Stasse's avatar Olivier Stasse
Browse files

Add mutex to joint state interface reading robot's state and writing

ctrl.
parent e933b727
No related branches found
No related tags found
No related merge requests found
#pragma once
/// Standard includes
#include <vector>
#include <string>
......@@ -8,13 +9,40 @@
namespace gz_transport_hw_tools {
class LastState {
public:
/// Time
int64_t time_sec_;
int64_t time_nsec_;
/// Position
std::vector<double> positions_;
/// Velocity
std::vector<double> velocities_;
void resize(std::size_t asize);
std::mutex lock_state_access_;
LastState();
};
class JointValues {
double pos_mes;
double vel_mes;
double force_ctrl;
std::string cmd_force_topic;
gz::transport::Node::Publisher;
};
/// This class handles the interface to the joints and the sensors
/// of a robot simulated by Gazebo (Harmonic)
class JointStateInterface {
public:
/// Constructor
JointStateInterface(std::string &a_prefix_model_root,
std::string &a_prefix_world);
std::string &a_prefix_world,
bool debug_level=false);
/// Destructor
~JointStateInterface();
......@@ -24,26 +52,27 @@ class JointStateInterface {
bool SetCmd(const std::vector<double> &cmd_vec);
void GetPosVel(std::vector<double> &pos_vecd,
std::vector<double> &vel_vecd);
bool GetPosVel(std::vector<double> &pos_vecd,
std::vector<double> &vel_vecd,
double &time);
private:
/// Callback function for state model update.
void CallbackJointState(const gz::msgs::Model &a_gz_model_msg);
/// Root of the prefix model
std::string prefix_model_root_;
/// Prefix world
std::string prefix_world_;
/// List of joints
std::vector<std::string> list_of_joints_;
/// Map joints to index in list
std::map<std::string, std::size_t> map_name_2_indx_;
/// Vector of string describing the topics to command forces on actuators.
std::vector<std::string> cmd_force_topics_;
......@@ -56,11 +85,10 @@ class JointStateInterface {
/// GZ node
gz::transport::Node node_;
/// Position
std::vector<double> positions_;
/// Last state of the robot
LastState last_state_;
/// Velocity
std::vector<double> velocities_;
bool debug_level_;
};
}
......@@ -4,8 +4,27 @@
namespace gz_transport_hw_tools {
LastState::LastState() :
time_sec_(0),
time_nsec_(0)
{}
void LastState::resize(size_t asize) {
positions_.clear();
positions_.resize(asize);
velocities_.clear();
velocities_.resize(asize);
for(unsigned i=0;i<positions_.size();i++)
{
positions_[i] = 0.0;
velocities_[i] = 0.0;
}
}
JointStateInterface::JointStateInterface(std::string &a_prefix_model_root,
std::string &a_prefix_world) {
std::string &a_prefix_world,
bool debug_level):
debug_level_(debug_level) {
prefix_model_root_ = a_prefix_model_root;
prefix_world_ = a_prefix_world ;
joint_state_topic_ =a_prefix_world + a_prefix_model_root + "joint_state";
......@@ -43,32 +62,48 @@ void JointStateInterface::SetListOfJoints(
map_name_2_indx_[*jointItr] = idx;
idx++;
}
positions_.clear();
positions_.resize(a_list_of_joints.size());
velocities_.clear();
velocities_.resize(a_list_of_joints.size());
last_state_.resize(a_list_of_joints.size());
}
void JointStateInterface::CallbackJointState(
const gz::msgs::Model&a_gz_model_msg
) {
std::lock_guard(last_state_.lock_state_access_);
unsigned int idx_joints=0;
for (auto jointItr = a_gz_model_msg.joint().begin();
jointItr != a_gz_model_msg.joint().end();
jointItr++) {
const ::gz::msgs::Axis &axis1 = jointItr->axis1();
long unsigned int local_joint_idx = map_name_2_indx_[jointItr->name()];
if (positions_.size()!=0)
positions_[local_joint_idx] = axis1.position();
if (velocities_.size()!=0)
velocities_[local_joint_idx] = axis1.velocity();
if (last_state_.positions_.size()!=0)
last_state_.positions_[local_joint_idx] = axis1.position();
if (last_state_.velocities_.size()!=0)
last_state_.velocities_[local_joint_idx] = axis1.velocity();
idx_joints++;
}
if (debug_level_)
{
std::cerr << "JointStateInterface::CallbackJointState: time: "
<< " " << last_state_.time_sec_
<< " " << last_state_.time_nsec_;
if (last_state_.positions_.size()>1)
std::cerr << " " << last_state_.positions_[1];
std::cerr << std::endl;
}
last_state_.time_sec_ = a_gz_model_msg.header().stamp().sec();
last_state_.time_nsec_ = a_gz_model_msg.header().stamp().nsec();
}
bool JointStateInterface::SetCmd( const std::vector<double> &a_cmd_vec_d)
......@@ -76,7 +111,8 @@ bool JointStateInterface::SetCmd( const std::vector<double> &a_cmd_vec_d)
if (a_cmd_vec_d.size()!=gz_pub_cmd_forces_.size())
return false;
for (unsigned int i = 0; i < a_cmd_vec_d.size(); i++) {
for (unsigned int i = 0; i < a_cmd_vec_d.size(); i++)
{
gz::msgs::Double msg;
msg.set_data(a_cmd_vec_d[i]);
......@@ -84,20 +120,37 @@ bool JointStateInterface::SetCmd( const std::vector<double> &a_cmd_vec_d)
std::cerr << "Unable to publish on "
<< cmd_force_topics_[i]
<< std::endl;
return false;
}
}
return true;
}
void JointStateInterface::GetPosVel(std::vector<double> &pos_vecd,
std::vector<double> &vel_vecd)
bool JointStateInterface::GetPosVel(std::vector<double> &pos_vecd,
std::vector<double> &vel_vecd,
double &time)
{
if ((pos_vecd.size()!= last_state_.positions_.size()) ||
(vel_vecd.size()!= last_state_.velocities_.size()))
return false;
std::lock_guard(last_state_.lock_state_access_);
time=last_state_.time_sec_ + 1e-9*last_state_.time_nsec_;
for(std::vector<double>::size_type idx=0;
idx < pos_vecd.size();
idx++)
{
pos_vecd[idx] = positions_[idx];
vel_vecd[idx] = velocities_[idx];
pos_vecd[idx] = last_state_.positions_[idx];
vel_vecd[idx] = last_state_.velocities_[idx];
}
if (debug_level_)
std::cerr << "JointStateInterface::GetPosVel: time: "
<< time << " "
<< pos_vecd[1] << " "
<< std::endl;
return true;
}
};
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment