Thread: [Personalrobots-commit] SF.net SVN: personalrobots:[4779] pkg/trunk (Page 11)

SourceForge Headquarters 1320 Columbia Street Suite 310 San Diego, CA 92101 +1 (858) 422-6466

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.




Thread: [Personalrobots-commit] SF.net SVN: personalrobots:[4779] pkg/trunk (Page 11)

personalrobots-commit