Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[4779] pkg/trunk
|
From: <stu...@us...> - 2008-09-29 20:18:10
|
Revision: 4779
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4779&view=rev
Author: stuglaser
Date: 2008-09-29 20:17:19 +0000 (Mon, 29 Sep 2008)
Log Message:
-----------
RealtimePublisher is now actually realtime safe.
Modified Paths:
--------------
pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/include/motor_qualification_controllers/motor_test1.h
pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/include/motor_qualification_controllers/motor_test2.h
pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/include/motor_qualification_controllers/motor_test3.h
pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/src/motor_test1.cpp
pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/src/motor_test2.cpp
pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/src/motor_test3.cpp
pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp
pkg/trunk/drivers/motor/ethercat_hardware/src/wg0x.cpp
pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
pkg/trunk/util/misc_utils/include/misc_utils/realtime_publisher.h
Modified: pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/include/motor_qualification_controllers/motor_test1.h
===================================================================
--- pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/include/motor_qualification_controllers/motor_test1.h 2008-09-29 20:03:36 UTC (rev 4778)
+++ pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/include/motor_qualification_controllers/motor_test1.h 2008-09-29 20:17:19 UTC (rev 4779)
@@ -38,7 +38,7 @@
/*! \class controller::MotorTest1
\brief Motor Test1 Controller
- This tests the motor enocder assembly to check if
+ This tests the motor enocder assembly to check if
the two are assembled properly.
*/
@@ -81,7 +81,7 @@
* \brief Get latest time..
*/
double getTime();
-
+
/*!
* \brief Perform the test analysis
*/
@@ -107,15 +107,13 @@
double fixture_joint_end_pos_; /**< End pos of the fixture joint. */
bool complete;
misc_utils::RealtimePublisher<robot_msgs::DiagnosticMessage> publisher_;
- robot_msgs::DiagnosticMessage diagnostic_message_;
-
};
/***************************************************/
/*! \class controller::MotorTest1Node
\brief Motor Test1 Controller
- This tests the motor enocder assembly to check if
+ This tests the motor enocder assembly to check if
the two are assembled properly.
*/
Modified: pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/include/motor_qualification_controllers/motor_test2.h
===================================================================
--- pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/include/motor_qualification_controllers/motor_test2.h 2008-09-29 20:03:36 UTC (rev 4778)
+++ pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/include/motor_qualification_controllers/motor_test2.h 2008-09-29 20:17:19 UTC (rev 4779)
@@ -38,7 +38,7 @@
/*! \class controller::MotorTest2
\brief Motor Test1 Controller
- This tests the motor enocder assembly to check if
+ This tests the motor enocder assembly to check if
the two are assembled properly.
*/
@@ -85,7 +85,7 @@
* \brief Get latest time..
*/
double getTime();
-
+
/*!
* \brief Perform the test analysis
*/
@@ -111,24 +111,20 @@
double initial_time_; /**< Start time of the test. */
bool complete;
misc_utils::RealtimePublisher<robot_msgs::DiagnosticMessage> publisher_;
- robot_msgs::DiagnosticMessage diagnostic_message_;
-
-
+
NEWMAT::ColumnVector test_voltage_;
- NEWMAT::ColumnVector test_velocity_;
+ NEWMAT::ColumnVector test_velocity_;
NEWMAT::ColumnVector test_current_;
NEWMAT::ColumnVector test_baseline_;
NEWMAT::UpperTriangularMatrix U_;
NEWMAT::ColumnVector M_;
-
-
};
/***************************************************/
/*! \class controller::MotorTest2Node
\brief Motor Test1 Controller
- This tests the motor enocder assembly to check if
+ This tests the motor enocder assembly to check if
the two are assembled properly.
*/
Modified: pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/include/motor_qualification_controllers/motor_test3.h
===================================================================
--- pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/include/motor_qualification_controllers/motor_test3.h 2008-09-29 20:03:36 UTC (rev 4778)
+++ pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/include/motor_qualification_controllers/motor_test3.h 2008-09-29 20:17:19 UTC (rev 4779)
@@ -38,7 +38,7 @@
/*! \class controller::MotorTest3
\brief Motor Test1 Controller
- This tests the motor enocder assembly to check if
+ This tests the motor enocder assembly to check if
the two are assembled properly.
*/
@@ -79,7 +79,7 @@
void init(double amplitude, std::string fixture_name, double time, std::string name ,mechanism::RobotState *robot);
bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
-
+
/*!
* \brief Perform the test analysis
*/
@@ -102,16 +102,13 @@
double initial_time_; /**< Start time of the test. */
bool complete;
misc_utils::RealtimePublisher<robot_msgs::DiagnosticMessage> publisher_;
- robot_msgs::DiagnosticMessage diagnostic_message_;
-
-
};
/***************************************************/
/*! \class controller::MotorTest3Node
\brief Motor Test1 Controller
- This tests the motor enocder assembly to check if
+ This tests the motor enocder assembly to check if
the two are assembled properly.
*/
Modified: pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/src/motor_test1.cpp
===================================================================
--- pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/src/motor_test1.cpp 2008-09-29 20:03:36 UTC (rev 4778)
+++ pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/src/motor_test1.cpp 2008-09-29 20:17:19 UTC (rev 4779)
@@ -43,7 +43,7 @@
{
robot_ = NULL;
joint_ = NULL;
-
+
duration_=0;
torque_=0;
initial_time_=0;
@@ -52,7 +52,7 @@
fixture_joint_end_pos_= 0;
test_joint_end_pos_= 0;
complete = false;
-
+
}
MotorTest1::~MotorTest1()
@@ -91,7 +91,7 @@
fprintf(stderr, "MotorTest1 could not find joint named \"%s\"\n", joint_name);
return false;
}
-
+
TiXmlElement *cd = j->FirstChildElement("controller_defaults");
if (cd)
{
@@ -116,7 +116,7 @@
void MotorTest1::update()
{
double time = robot_->hw_->current_time_;
-
+
if((time-initial_time_)<duration_)
{
joint_->commanded_effort_ = torque_;
@@ -132,7 +132,7 @@
analysis();
complete = true;
}
- else
+ else
return;
}
@@ -142,8 +142,12 @@
double t_delta = test_joint_end_pos_-test_joint_start_pos_;
double error = fabs(fabs(f_delta)-fabs(t_delta))/((fabs(f_delta)+fabs(t_delta))/2);
printf("f: %f , t: %f\n", f_delta, t_delta);
- diagnostic_message_.set_status_size(1);
- robot_msgs::DiagnosticStatus *status = diagnostic_message_.status;
+
+ // Screw realtime
+ publisher_.lock();
+
+ publisher_.msg_.set_status_size(1);
+ robot_msgs::DiagnosticStatus *status = publisher_.msg_.status;
status->name = "MotorTest";
printf("error: %f\n", error);
if (f_delta==0 && t_delta==0)
@@ -157,7 +161,7 @@
//the motor doesn't have an encoder
status->level = 2;
status->message = "ERROR: The motor encoder is not attached or not powered.";
-
+
}
else if(error<0.005)
{
@@ -167,23 +171,24 @@
}
else if(f_delta>0 && t_delta<0)
{
- //the encoder is reversed
+ //the encoder is reversed
status->level = 2;
status->message = "ERROR: The motor encoder is reversed.";
}
- else if(f_delta<0 && t_delta<0)
+ else if(f_delta<0 && t_delta<0)
{
//motor reversed
status->level = 2;
status->message = "ERROR: The motor wiring is reversed.";
- }
+ }
else
{
//test passed
status->level = 0;
status->message = "OK: Passed.";
- }
- publisher_.publish(diagnostic_message_);
+ }
+
+ publisher_.unlockAndPublish();
return;
}
Modified: pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/src/motor_test2.cpp
===================================================================
--- pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/src/motor_test2.cpp 2008-09-29 20:03:36 UTC (rev 4778)
+++ pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/src/motor_test2.cpp 2008-09-29 20:17:19 UTC (rev 4779)
@@ -45,7 +45,7 @@
robot_ = NULL;
actuator_ = NULL;
joint_ = NULL;
-
+
count_=1;
duration_=4;
torque_=0;
@@ -53,8 +53,8 @@
resistance_=10;
initial_time_=0;
complete = false;
-
-
+
+
}
MotorTest2::~MotorTest2()
@@ -94,7 +94,7 @@
fprintf(stderr, "MotorTest2 could not find joint named \"%s\"\n", joint_name);
return false;
}
-
+
TiXmlElement *cd = j->FirstChildElement("controller_defaults");
if (cd)
{
@@ -123,11 +123,11 @@
double time = robot_->hw_->current_time_;
static int first_time = 1;
static double zero_offset = 0;
-
-
+
+
if ((time-initial_time_)<duration_) {
- test_baseline_(count_) = actuator_->state_.motor_voltage_;
-
+ test_baseline_(count_) = actuator_->state_.motor_voltage_;
+
count_++;
}
else if((time-initial_time_)<2*duration_)
@@ -145,35 +145,38 @@
else if((time-initial_time_)<3*duration_)
{
joint_->commanded_effort_ = torque_;
- test_voltage_(count_) =actuator_->state_.motor_voltage_- zero_offset;
+ test_voltage_(count_) =actuator_->state_.motor_voltage_- zero_offset;
test_current_(count_) = actuator_->state_.last_measured_current_;
test_velocity_(count_) =joint_->velocity_;
count_++;
}
else if (!complete)
{
- joint_->commanded_effort_ = 0.0;
+ joint_->commanded_effort_ = 0.0;
analysis();
complete = true;
}
- else
+ else
return;
}
void MotorTest2::analysis()
{
- diagnostic_message_.set_status_size(1);
- robot_msgs::DiagnosticStatus *status = diagnostic_message_.status;
+ // Screw realtime
+ publisher_.lock();
+
+ publisher_.msg_.set_status_size(1);
+ robot_msgs::DiagnosticStatus *status = publisher_.msg_.status;
status->set_values_size(2);
status->name = "MotorTest";
NEWMAT::Matrix test_matrix =test_velocity_ | test_current_;
- QRZ(test_matrix, U_);
+ QRZ(test_matrix, U_);
QRZ(test_matrix, test_voltage_, M_);
NEWMAT::Matrix solution=U_.i()*M_;
double speed_const_meas = fabs(1/solution(1,1));
//ouble resistance_meas =fabs(solution(2,1)); this was all over the place no idea why??
-
+
if (fabs(speed_const_meas-speed_constant_)/speed_constant_ > 0.05)
{
//the motor isn't moving
@@ -194,8 +197,10 @@
status->values[0].value = speed_const_meas;
status->values[1].label = "expected speed constant";
status->values[1].value = speed_constant_;
- }
- publisher_.publish(diagnostic_message_);
+ }
+
+ publisher_.unlockAndPublish();
+
return;
}
Modified: pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/src/motor_test3.cpp
===================================================================
--- pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/src/motor_test3.cpp 2008-09-29 20:03:36 UTC (rev 4778)
+++ pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/src/motor_test3.cpp 2008-09-29 20:17:19 UTC (rev 4779)
@@ -44,14 +44,14 @@
robot_ = NULL;
joint_ = NULL;
-
-
+
+
duration_=20;
amplitude_=0;
initial_time_=0;
complete = false;
-
+
}
MotorTest3::~MotorTest3()
@@ -66,7 +66,7 @@
duration_=20;
sweep_controller_ = new SineSweepController();
sweep_controller_->init(1.0, 40, duration_, amplitude,robot->hw_->current_time_ ,name,robot);
-
+
amplitude_=amplitude;
initial_time_=time;
@@ -91,10 +91,10 @@
fprintf(stderr, "MotorTest3 could not find joint named \"%s\"\n", joint_name);
return false;
}
-
+
TiXmlElement *cd = j->FirstChildElement("controller_defaults");
if (cd)
- {
+ {
double amplitude = atof(cd->Attribute("amplitude"));
std::string fixture_name = cd->Attribute("fixture_name");
init(amplitude, fixture_name, robot->hw_->current_time_,j->Attribute("name"), robot);
@@ -111,34 +111,35 @@
void MotorTest3::update()
{
double time = robot_->hw_->current_time_;
-
-
+
+
if((time-initial_time_)<duration_)
{
sweep_controller_->update();
}
else if (!complete)
{
-
+
analysis();
complete = true;
}
- else
+ else
return;
}
void MotorTest3::analysis()
{
- diagnostic_message_.set_status_size(1);
- robot_msgs::DiagnosticStatus *status = diagnostic_message_.status;
+ publisher_.lock(); // Screw realtime
+ publisher_.msg_.set_status_size(1);
+ robot_msgs::DiagnosticStatus *status = publisher_.msg_.status;
status->name = "MotorTest";
-
+
//test passed
status->level = 0;
status->message = "OK: Passed.";
- publisher_.publish(diagnostic_message_);
+ publisher_.unlockAndPublish();
return;
}
Modified: pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp 2008-09-29 20:03:36 UTC (rev 4778)
+++ pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp 2008-09-29 20:17:19 UTC (rev 4779)
@@ -236,8 +236,11 @@
}
// Publish status of each EtherCAT device
- diagnostic_message_.set_status_vec(statuses);
- publisher_.publish(diagnostic_message_);
+ if (publisher_.trylock())
+ {
+ publisher_.msg_.set_status_vec(statuses);
+ publisher_.unlockAndPublish();
+ }
}
void EthercatHardware::update()
Modified: pkg/trunk/drivers/motor/ethercat_hardware/src/wg0x.cpp
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/src/wg0x.cpp 2008-09-29 20:03:36 UTC (rev 4778)
+++ pkg/trunk/drivers/motor/ethercat_hardware/src/wg0x.cpp 2008-09-29 20:17:19 UTC (rev 4779)
@@ -196,7 +196,7 @@
unsigned int minor = revision & 0xff;
printf("Device #%02d: WG0%d (%#08x) Firmware Revision %d.%02d, PCB Revision %c.%02d\n", sh_->get_ring_position(),
- sh_->get_product_code() == WG05::PRODUCT_CODE ? 5 : 6,
+ sh_->get_product_code() == WG05::PRODUCT_CODE ? 5 : 6,
sh_->get_product_code(), major, minor,
'A' + ((revision >> 24) & 0xff) - 1, (revision >> 16) & 0xff);
@@ -323,14 +323,16 @@
if (p->timestamp_ != last_pressure_time_)
{
- ethercat_hardware::PressureState pressure_;
- pressure_.set_data0_size(22);
- pressure_.set_data1_size(22);
- for (int i = 0; i < 22; ++i ) {
- pressure_.data0[i] = ((p->data0_[i] >> 8) & 0xff) | ((p->data0_[i] << 8) & 0xff00);
- pressure_.data1[i] = ((p->data1_[i] >> 8) & 0xff) | ((p->data1_[i] << 8) & 0xff00);
+ if (publisher_.trylock())
+ {
+ publisher_.msg_.set_data0_size(22);
+ publisher_.msg_.set_data1_size(22);
+ for (int i = 0; i < 22; ++i ) {
+ publisher_.msg_.data0[i] = ((p->data0_[i] >> 8) & 0xff) | ((p->data0_[i] << 8) & 0xff00);
+ publisher_.msg_.data1[i] = ((p->data1_[i] >> 8) & 0xff) | ((p->data1_[i] << 8) & 0xff00);
+ }
+ publisher_.unlockAndPublish();
}
- publisher_.publish(pressure_);
}
WG0X::convertState(state, current_buffer, last_buffer);
Modified: pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
===================================================================
--- pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h 2008-09-29 20:03:36 UTC (rev 4778)
+++ pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h 2008-09-29 20:17:19 UTC (rev 4779)
@@ -123,15 +123,10 @@
static const double STATE_PUBLISHING_PERIOD = 0.01; // this translates to about 100Hz
- mechanism_control::MechanismState mechanism_state_;
const char* const mechanism_state_topic_;
misc_utils::RealtimePublisher<mechanism_control::MechanismState> publisher_;
- rosTF::TransformArray transform_array_msg_;
- misc_utils::RealtimePublisher<rosTF::TransformArray> transform_array_publisher_;
-
- rostools::Time time_msg_;
- misc_utils::RealtimePublisher<rostools::Time> time_publisher_;
+ misc_utils::RealtimePublisher<rosTF::TransformArray> transform_publisher_;
};
#endif /* MECHANISM_CONTROL_H */
Modified: pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-09-29 20:03:36 UTC (rev 4778)
+++ pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-09-29 20:17:19 UTC (rev 4779)
@@ -145,7 +145,7 @@
controller::Controller *c = controller::ControllerFactory::instance().create(type);
if (c == NULL)
return false;
- printf("Spawning %s: %08x\n", name.c_str(), &model_);
+ printf("Spawning %s: %08x\n", name.c_str(), (unsigned int)&model_);
if (!c->initXml(state_, config) ||
@@ -189,7 +189,7 @@
MechanismControlNode::MechanismControlNode(MechanismControl *mc)
: mc_(mc), mechanism_state_topic_("mechanism_state"), publisher_(mechanism_state_topic_, 1),
- transform_array_publisher_("TransformArray", 1), time_publisher_("time", 1)
+ transform_publisher_("TransformArray", 1)
{
assert(mc != NULL);
assert(mechanism_state_topic_);
@@ -229,9 +229,9 @@
{
if (!mc_->initXml(config))
return false;
- mechanism_state_.set_joint_states_size(mc_->model_.joints_.size());
- mechanism_state_.set_actuator_states_size(mc_->hw_->actuators_.size());
- transform_array_msg_.set_quaternions_size(mc_->model_.links_.size());
+ publisher_.msg_.set_joint_states_size(mc_->model_.joints_.size());
+ publisher_.msg_.set_actuator_states_size(mc_->hw_->actuators_.size());
+ transform_publisher_.msg_.set_quaternions_size(mc_->model_.links_.size());
return true;
}
@@ -242,77 +242,80 @@
static double last_publish_time = 0.0;
if (mc_->hw_->current_time_ - last_publish_time > STATE_PUBLISHING_PERIOD)
{
- assert(mc_->model_.joints_.size() == mechanism_state_.get_joint_states_size());
- for (unsigned int i = 0; i < mc_->model_.joints_.size(); ++i)
+ if (publisher_.trylock())
{
- mechanism_control::JointState *out = &mechanism_state_.joint_states[i];
- mechanism::JointState *in = &mc_->state_->joint_states_[i];
- out->name = mc_->model_.joints_[i]->name_;
- out->position = in->position_;
- out->velocity = in->velocity_;
- out->applied_effort = in->applied_effort_;
- out->commanded_effort = in->commanded_effort_;
- }
+ assert(mc_->model_.joints_.size() == publisher_.msg_.get_joint_states_size());
+ for (unsigned int i = 0; i < mc_->model_.joints_.size(); ++i)
+ {
+ mechanism_control::JointState *out = publisher_.msg_.joint_states + i;
+ mechanism::JointState *in = &mc_->state_->joint_states_[i];
+ out->name = mc_->model_.joints_[i]->name_;
+ out->position = in->position_;
+ out->velocity = in->velocity_;
+ out->applied_effort = in->applied_effort_;
+ out->commanded_effort = in->commanded_effort_;
+ }
- for (unsigned int i = 0; i < mc_->hw_->actuators_.size(); ++i)
- {
- mechanism_control::ActuatorState *out = &mechanism_state_.actuator_states[i];
- ActuatorState *in = &mc_->hw_->actuators_[i]->state_;
- out->name = mc_->hw_->actuators_[i]->name_;
- out->encoder_count = in->encoder_count_;
- out->position = in->position_;
- out->timestamp = in->timestamp_;
- out->encoder_velocity = in->encoder_velocity_;
- out->velocity = in->velocity_;
- out->calibration_reading = in->calibration_reading_;
- out->last_calibration_high_transition = in->last_calibration_high_transition_;
- out->last_calibration_low_transition = in->last_calibration_low_transition_;
- out->is_enabled = in->is_enabled_;
- out->run_stop_hit = in->run_stop_hit_;
- out->last_requested_current = in->last_requested_current_;
- out->last_commanded_current = in->last_commanded_current_;
- out->last_measured_current = in->last_measured_current_;
- out->last_requested_effort = in->last_requested_effort_;
- out->last_commanded_effort = in->last_commanded_effort_;
- out->last_measured_effort = in->last_measured_effort_;
- out->motor_voltage = in->motor_voltage_;
- out->num_encoder_errors = in->num_encoder_errors_;
+ for (unsigned int i = 0; i < mc_->hw_->actuators_.size(); ++i)
+ {
+ mechanism_control::ActuatorState *out = publisher_.msg_.actuator_states + i;
+ ActuatorState *in = &mc_->hw_->actuators_[i]->state_;
+ out->name = mc_->hw_->actuators_[i]->name_;
+ out->encoder_count = in->encoder_count_;
+ out->position = in->position_;
+ out->timestamp = in->timestamp_;
+ out->encoder_velocity = in->encoder_velocity_;
+ out->velocity = in->velocity_;
+ out->calibration_reading = in->calibration_reading_;
+ out->last_calibration_high_transition = in->last_calibration_high_transition_;
+ out->last_calibration_low_transition = in->last_calibration_low_transition_;
+ out->is_enabled = in->is_enabled_;
+ out->run_stop_hit = in->run_stop_hit_;
+ out->last_requested_current = in->last_requested_current_;
+ out->last_commanded_current = in->last_commanded_current_;
+ out->last_measured_current = in->last_measured_current_;
+ out->last_requested_effort = in->last_requested_effort_;
+ out->last_commanded_effort = in->last_commanded_effort_;
+ out->last_measured_effort = in->last_measured_effort_;
+ out->motor_voltage = in->motor_voltage_;
+ out->num_encoder_errors = in->num_encoder_errors_;
+ }
+ publisher_.msg_.time = mc_->hw_->current_time_;
+
+ publisher_.unlockAndPublish();
}
- mechanism_state_.time = mc_->hw_->current_time_;
- publisher_.publish(mechanism_state_);
-
// Frame transforms
- assert(mc_->model_.links_.size() == transform_array_msg_.get_quaternions_size());
- for (unsigned int i = 0; i < mc_->model_.links_.size(); ++i)
+ if (transform_publisher_.trylock())
{
- if (mc_->model_.links_[i]->parent_name_ == std::string("world"))
- continue;
+ assert(mc_->model_.links_.size() == transform_publisher_.msg_.get_quaternions_size());
+ for (unsigned int i = 0; i < mc_->model_.links_.size(); ++i)
+ {
+ if (mc_->model_.links_[i]->parent_name_ == std::string("world"))
+ continue;
- libTF::Position pos;
- libTF::Quaternion quat;
- mc_->state_->link_states_[i].rel_frame_.getPosition(pos);
- mc_->state_->link_states_[i].rel_frame_.getQuaternion(quat);
- rosTF::TransformQuaternion &out = transform_array_msg_.quaternions[i];
+ libTF::Position pos;
+ libTF::Quaternion quat;
+ mc_->state_->link_states_[i].rel_frame_.getPosition(pos);
+ mc_->state_->link_states_[i].rel_frame_.getQuaternion(quat);
+ rosTF::TransformQuaternion &out = transform_publisher_.msg_.quaternions[i];
- out.header.stamp.from_double(mc_->hw_->current_time_);
- out.header.frame_id = mc_->model_.links_[i]->name_;
- out.parent = mc_->model_.links_[i]->parent_name_;
- out.xt = pos.x;
- out.yt = pos.y;
- out.zt = pos.z;
- out.w = quat.w;
- out.xr = quat.x;
- out.yr = quat.y;
- out.zr = quat.z;
+ out.header.stamp.from_double(mc_->hw_->current_time_);
+ out.header.frame_id = mc_->model_.links_[i]->name_;
+ out.parent = mc_->model_.links_[i]->parent_name_;
+ out.xt = pos.x;
+ out.yt = pos.y;
+ out.zt = pos.z;
+ out.w = quat.w;
+ out.xr = quat.x;
+ out.yr = quat.y;
+ out.zr = quat.z;
+ }
+
+ transform_publisher_.unlockAndPublish();
}
- transform_array_publisher_.publish(transform_array_msg_);
- // rostime
- time_msg_.rostime.from_double(mc_->hw_->current_time_);
- time_publisher_.publish(time_msg_);
-
last_publish_time = mc_->hw_->current_time_;
}
}
Modified: pkg/trunk/util/misc_utils/include/misc_utils/realtime_publisher.h
===================================================================
--- pkg/trunk/util/misc_utils/include/misc_utils/realtime_publisher.h 2008-09-29 20:03:36 UTC (rev 4778)
+++ pkg/trunk/util/misc_utils/include/misc_utils/realtime_publisher.h 2008-09-29 20:17:19 UTC (rev 4779)
@@ -50,7 +50,7 @@
{
public:
RealtimePublisher(const std::string &topic, int queue_size)
- : topic_(topic), node_(NULL), is_running_(false), keep_running_(false), thread_(NULL)
+ : topic_(topic), node_(NULL), is_running_(false), keep_running_(false), thread_(NULL), turn_(REALTIME)
{
if ((node_ = ros::node::instance()) == NULL)
{
@@ -70,6 +70,7 @@
~RealtimePublisher()
{
+ node_->unadvertise(topic_);
}
void stop()
@@ -77,31 +78,57 @@
keep_running_ = false;
}
- void publish(const Msg &msg)
+ Msg msg_;
+
+ void lock()
{
+ pthread_mutex_lock(&msg_lock_);
+ }
+
+ bool trylock()
+ {
if (0 == pthread_mutex_trylock(&msg_lock_))
{
- msg_ = msg;
- msg_updated_ = true;
- pthread_mutex_unlock(&msg_lock_);
- pthread_cond_signal(&updated_cond_);
+ if (turn_ == REALTIME)
+ {
+ return true;
+ }
+ else
+ {
+ pthread_mutex_unlock(&msg_lock_);
+ return false;
+ }
}
+ else
+ return false;
}
+ void unlock()
+ {
+ pthread_mutex_unlock(&msg_lock_);
+ }
+
+ void unlockAndPublish()
+ {
+ turn_ = NON_REALTIME;
+ pthread_mutex_unlock(&msg_lock_);
+ pthread_cond_signal(&updated_cond_);
+ }
+
bool is_running() const { return is_running_; }
void publishingLoop()
{
is_running_ = true;
- msg_updated_ = false;
+ turn_ = REALTIME;
while (keep_running_)
{
// Locks msg_ and copies it
pthread_mutex_lock(&msg_lock_);
- while (!msg_updated_)
+ while (turn_ != NON_REALTIME)
pthread_cond_wait(&updated_cond_, &msg_lock_);
Msg outgoing(msg_);
- msg_updated_ = false;
+ turn_ = REALTIME;
pthread_mutex_unlock(&msg_lock_);
// Sends the outgoing message
@@ -111,6 +138,7 @@
}
private:
+
std::string topic_;
ros::node *node_;
bool is_running_;
@@ -118,10 +146,11 @@
pthread_t *thread_;
- Msg msg_;
- bool msg_updated_;
pthread_mutex_t msg_lock_; // Protects msg_
pthread_cond_t updated_cond_;
+
+ enum {REALTIME, NON_REALTIME};
+ int turn_; // Who's turn is it to use msg_?
};
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
