Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[21034] pkg/trunk
|
From: <rob...@us...> - 2009-08-07 18:38:35
|
Revision: 21034
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=21034&view=rev
Author: rob_wheeler
Date: 2009-08-07 18:38:25 +0000 (Fri, 07 Aug 2009)
Log Message:
-----------
Kill old realtime_tools.h wrapper for Xenomai.
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/joint_trajectory_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/joint_trajectory_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_position_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_velocity_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/probe.h
pkg/trunk/drivers/cam/camera_trigger_test/src/led_flash_record.cpp
pkg/trunk/drivers/cam/camera_trigger_test/src/timestamp_test.cpp
pkg/trunk/drivers/cam/camera_trigger_test/src/trigger_test.cpp
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/pid_position_velocity_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/trajectory_controller.h
pkg/trunk/stacks/mechanism/mechanism_control/include/mechanism_control/recorder.h
pkg/trunk/stacks/mechanism/mechanism_control/src/mechanism_control.cpp
pkg/trunk/stacks/mechanism/mechanism_control/src/recorder.cpp
pkg/trunk/stacks/pr2_ethercat_drivers/ethercat_hardware/src/ethercat_hardware.cpp
pkg/trunk/stacks/pr2_ethercat_drivers/ethercat_hardware/src/wg0x.cpp
pkg/trunk/util/realtime_tools/CMakeLists.txt
pkg/trunk/util/realtime_tools/include/realtime_tools/realtime_publisher.h
pkg/trunk/util/realtime_tools/include/realtime_tools/realtime_srv_call.h
Removed Paths:
-------------
pkg/trunk/util/realtime_tools/include/realtime_tools/realtime_tools.h
pkg/trunk/util/realtime_tools/include/realtime_tools/xenomai_prototypes.h
pkg/trunk/util/realtime_tools/src/
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h 2009-08-07 18:26:25 UTC (rev 21033)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h 2009-08-07 18:38:25 UTC (rev 21034)
@@ -68,7 +68,6 @@
#include <pr2_mechanism_controllers/ControllerState.h>
#include <realtime_tools/realtime_publisher.h>
-#include <realtime_tools/realtime_tools.h>
#include <std_msgs/String.h>
namespace controller
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/joint_trajectory_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/joint_trajectory_controller.h 2009-08-07 18:26:25 UTC (rev 21033)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/joint_trajectory_controller.h 2009-08-07 18:38:25 UTC (rev 21034)
@@ -54,7 +54,6 @@
#include <pr2_mechanism_controllers/ControllerState.h>
#include <realtime_tools/realtime_publisher.h>
-#include <realtime_tools/realtime_tools.h>
#include <std_msgs/String.h>
#include <angles/angles.h>
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp 2009-08-07 18:26:25 UTC (rev 21033)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp 2009-08-07 18:38:25 UTC (rev 21034)
@@ -191,7 +191,7 @@
{
#ifdef PUBLISH_MAX_TIME
- double start_time = realtime_gettime();
+ double start_time = ros::Time::now().toSec();
#endif
double sample_time(0.0);
@@ -238,7 +238,7 @@
updateJointControllers();
#ifdef PUBLISH_MAX_TIME
- double end_time = realtime_gettime();
+ double end_time = ros::Time::now().toSec();
max_update_time_ = std::max(max_update_time_,end_time-start_time);
if (controller_state_publisher_->trylock())
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/joint_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/joint_trajectory_controller.cpp 2009-08-07 18:26:25 UTC (rev 21033)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/joint_trajectory_controller.cpp 2009-08-07 18:38:25 UTC (rev 21034)
@@ -460,7 +460,7 @@
void JointTrajectoryController::update(void)
{
#ifdef PUBLISH_MAX_TIME
- double start_time = realtime_gettime();
+ double start_time = ros::Time::now().toSec();
#endif
current_time_ = robot_->hw_->current_time_;
updateJointValues();
@@ -531,7 +531,7 @@
#ifdef PUBLISH_MAX_TIME
- double end_time = realtime_gettime();
+ double end_time = ros::Time::now().toSec();
max_update_time_ = std::max(max_update_time_,end_time-start_time);
if (controller_state_publisher_->trylock())
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_position_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_position_controller.h 2009-08-07 18:26:25 UTC (rev 21033)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_position_controller.h 2009-08-07 18:38:25 UTC (rev 21034)
@@ -67,7 +67,6 @@
#include <robot_mechanism_controllers/JointControllerState.h>
#include <robot_mechanism_controllers/JointTuningMsg.h>
#include <realtime_tools/realtime_publisher.h>
-#include <realtime_tools/realtime_tools.h>
namespace controller
{
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_velocity_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_velocity_controller.h 2009-08-07 18:26:25 UTC (rev 21033)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_velocity_controller.h 2009-08-07 18:38:25 UTC (rev 21034)
@@ -75,7 +75,6 @@
//Realtime publisher
#include <robot_mechanism_controllers/JointControllerState.h>
#include <realtime_tools/realtime_publisher.h>
-#include <realtime_tools/realtime_tools.h>
namespace controller
{
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/probe.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/probe.h 2009-08-07 18:26:25 UTC (rev 21033)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/probe.h 2009-08-07 18:38:25 UTC (rev 21034)
@@ -39,8 +39,6 @@
#include <robot_mechanism_controllers/JointControllerState.h>
#include <robot_mechanism_controllers/JointTuningMsg.h>
-#include <realtime_tools/realtime_publisher.h>
-#include <realtime_tools/realtime_tools.h>
namespace controller
{
Modified: pkg/trunk/drivers/cam/camera_trigger_test/src/led_flash_record.cpp
===================================================================
--- pkg/trunk/drivers/cam/camera_trigger_test/src/led_flash_record.cpp 2009-08-07 18:26:25 UTC (rev 21033)
+++ pkg/trunk/drivers/cam/camera_trigger_test/src/led_flash_record.cpp 2009-08-07 18:38:25 UTC (rev 21034)
@@ -38,7 +38,6 @@
#include <stdio.h>
#include <signal.h>
#include <robot_mechanism_controllers/SetWaveform.h>
-#include <realtime_tools/realtime_tools.h>
#include <robot_mechanism_controllers/trigger_controller.h>
#include <algorithm>
#include <boost/format.hpp>
Modified: pkg/trunk/drivers/cam/camera_trigger_test/src/timestamp_test.cpp
===================================================================
--- pkg/trunk/drivers/cam/camera_trigger_test/src/timestamp_test.cpp 2009-08-07 18:26:25 UTC (rev 21033)
+++ pkg/trunk/drivers/cam/camera_trigger_test/src/timestamp_test.cpp 2009-08-07 18:38:25 UTC (rev 21034)
@@ -38,7 +38,6 @@
#include <stdio.h>
#include <signal.h>
#include <robot_mechanism_controllers/SetWaveform.h>
-#include <realtime_tools/realtime_tools.h>
#include <robot_mechanism_controllers/trigger_controller.h>
class TimestampTest
Modified: pkg/trunk/drivers/cam/camera_trigger_test/src/trigger_test.cpp
===================================================================
--- pkg/trunk/drivers/cam/camera_trigger_test/src/trigger_test.cpp 2009-08-07 18:26:25 UTC (rev 21033)
+++ pkg/trunk/drivers/cam/camera_trigger_test/src/trigger_test.cpp 2009-08-07 18:38:25 UTC (rev 21034)
@@ -38,7 +38,6 @@
#include <stdio.h>
#include <signal.h>
#include <robot_mechanism_controllers/SetWaveform.h>
-#include <realtime_tools/realtime_tools.h>
class TriggerTest
{
@@ -208,7 +207,7 @@
break;
case 1:
- double curtime = realtime_gettime();
+ double curtime = ros::Time::now().toSec();
double curphase = fmod(curtime * led_config_.rep_rate - led_config_.phase, 1);
fprintf(outfile_, "delay: %f intensity: %f led: %i\n", curphase / led_config_.rep_rate,
intensity, curphase < led_config_.duty_cycle);
Modified: pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/pid_position_velocity_controller.h
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/pid_position_velocity_controller.h 2009-08-07 18:26:25 UTC (rev 21033)
+++ pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/pid_position_velocity_controller.h 2009-08-07 18:38:25 UTC (rev 21034)
@@ -75,7 +75,6 @@
//Realtime publisher
#include <robot_mechanism_controllers/JointControllerState.h>
#include <realtime_tools/realtime_publisher.h>
-#include <realtime_tools/realtime_tools.h>
#include <angles/angles.h>
Modified: pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/trajectory_controller.h
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/trajectory_controller.h 2009-08-07 18:26:25 UTC (rev 21033)
+++ pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/trajectory_controller.h 2009-08-07 18:38:25 UTC (rev 21034)
@@ -47,7 +47,6 @@
#include <manipulation_srvs/QuerySplineTraj.h>
#include <manipulation_srvs/CancelSplineTraj.h>
-#include <realtime_tools/realtime_tools.h>
#include <realtime_tools/realtime_publisher.h>
#include <experimental_controllers/pid_position_velocity_controller.h>
Modified: pkg/trunk/stacks/mechanism/mechanism_control/include/mechanism_control/recorder.h
===================================================================
--- pkg/trunk/stacks/mechanism/mechanism_control/include/mechanism_control/recorder.h 2009-08-07 18:26:25 UTC (rev 21033)
+++ pkg/trunk/stacks/mechanism/mechanism_control/include/mechanism_control/recorder.h 2009-08-07 18:38:25 UTC (rev 21034)
@@ -37,7 +37,6 @@
#include "ros/node_handle.h"
#include "mechanism_model/robot.h"
-#include "realtime_tools/realtime_tools.h"
#include "mechanism_msgs/BufferedData.h"
Modified: pkg/trunk/stacks/mechanism/mechanism_control/src/mechanism_control.cpp
===================================================================
--- pkg/trunk/stacks/mechanism/mechanism_control/src/mechanism_control.cpp 2009-08-07 18:26:25 UTC (rev 21033)
+++ pkg/trunk/stacks/mechanism/mechanism_control/src/mechanism_control.cpp 2009-08-07 18:38:25 UTC (rev 21034)
@@ -59,8 +59,8 @@
pub_diagnostics_(node_, "/diagnostics", 1),
pub_joints_(node_, "joint_states", 1),
pub_mech_state_(node_, "mechanism_state", 1),
- last_published_state_(realtime_gettime()),
- last_published_diagnostics_(realtime_gettime())
+ last_published_state_(ros::Time::now().toSec()),
+ last_published_diagnostics_(ros::Time::now().toSec())
{
model_.hw_ = hw;
}
@@ -125,25 +125,25 @@
std::vector<ControllerSpec> &controllers = controllers_lists_[used_by_realtime_];
std::vector<size_t> &scheduling = controllers_scheduling_[used_by_realtime_];
- double start = realtime_gettime();
+ double start = ros::Time::now().toSec();
state_->propagateState();
state_->zeroCommands();
- double start_update = realtime_gettime();
+ double start_update = ros::Time::now().toSec();
pre_update_stats_.acc(start_update - start);
// Update all controllers in scheduling order
for (size_t i=0; i<controllers.size(); i++){
- double start = realtime_gettime();
+ double start = ros::Time::now().toSec();
controllers[scheduling[i]].c->updateRequest();
- double end = realtime_gettime();
+ double end = ros::Time::now().toSec();
controllers[scheduling[i]].stats->acc(end - start);
}
- double end_update = realtime_gettime();
+ double end_update = ros::Time::now().toSec();
update_stats_.acc(end_update - start_update);
state_->enforceSafety();
state_->propagateEffort();
- double end = realtime_gettime();
+ double end = ros::Time::now().toSec();
post_update_stats_.acc(end - end_update);
// publish diagnostics and state
@@ -441,9 +441,9 @@
void MechanismControl::publishDiagnostics()
{
- if (round ((realtime_gettime() - last_published_diagnostics_ - publish_period_diagnostics_)/ (0.000001)) >= 0)
+ if (round ((ros::Time::now().toSec() - last_published_diagnostics_ - publish_period_diagnostics_)/ (0.000001)) >= 0)
{
- last_published_diagnostics_ = realtime_gettime();
+ last_published_diagnostics_ = ros::Time::now().toSec();
if (pub_diagnostics_.trylock())
{
std::vector<ControllerSpec> &controllers = controllers_lists_[used_by_realtime_];
@@ -516,9 +516,9 @@
void MechanismControl::publishState()
{
- if (round ((realtime_gettime() - last_published_state_ - publish_period_state_)/ (0.000001)) >= 0)
+ if (round ((ros::Time::now().toSec() - last_published_state_ - publish_period_state_)/ (0.000001)) >= 0)
{
- last_published_state_ = realtime_gettime();
+ last_published_state_ = ros::Time::now().toSec();
if (pub_mech_state_.trylock())
{
unsigned int j = 0;
@@ -566,8 +566,8 @@
out->motor_voltage = in->motor_voltage_;
out->num_encoder_errors = in->num_encoder_errors_;
}
- pub_mech_state_.msg_.header.stamp.fromSec(realtime_gettime());
- pub_mech_state_.msg_.time = realtime_gettime();
+ pub_mech_state_.msg_.header.stamp = ros::Time::now();
+ pub_mech_state_.msg_.time = ros::Time::now().toSec();
pub_mech_state_.unlockAndPublish();
}
@@ -592,7 +592,7 @@
}
}
- pub_joints_.msg_.header.stamp.fromSec(realtime_gettime());
+ pub_joints_.msg_.header.stamp = ros::Time::now();
pub_joints_.unlockAndPublish();
}
Modified: pkg/trunk/stacks/mechanism/mechanism_control/src/recorder.cpp
===================================================================
--- pkg/trunk/stacks/mechanism/mechanism_control/src/recorder.cpp 2009-08-07 18:26:25 UTC (rev 21033)
+++ pkg/trunk/stacks/mechanism/mechanism_control/src/recorder.cpp 2009-08-07 18:38:25 UTC (rev 21034)
@@ -100,11 +100,6 @@
void Recorder::publishingLoop()
{
- RealtimeTask task;
- int err = realtime_shadow_task(&task);
- if (err)
- ROS_WARN("Unable to shadow task: %d\n", err);
-
ROS_DEBUG("Entering publishing loop (namespace: %s)", node_.getNamespace().c_str());
is_running_ = true;
while (keep_running_)
Modified: pkg/trunk/stacks/pr2_ethercat_drivers/ethercat_hardware/src/ethercat_hardware.cpp
===================================================================
--- pkg/trunk/stacks/pr2_ethercat_drivers/ethercat_hardware/src/ethercat_hardware.cpp 2009-08-07 18:26:25 UTC (rev 21033)
+++ pkg/trunk/stacks/pr2_ethercat_drivers/ethercat_hardware/src/ethercat_hardware.cpp 2009-08-07 18:38:25 UTC (rev 21034)
@@ -154,7 +154,7 @@
// Create HardwareInterface
hw_ = new HardwareInterface(num_actuators);
- hw_->current_time_ = realtime_gettime();
+ hw_->current_time_ = ros::Time::now().toSec();
last_published_ = hw_->current_time_;
// Initialize slaves
@@ -305,11 +305,11 @@
}
// Transmit process data
- double start = realtime_gettime();
+ double start = ros::Time::now().toSec();
if (!em_->txandrx_PD(buffer_size_, current_buffer_)) {
++diagnostics_.txandrx_errors_;
}
- diagnostics_.acc_(realtime_gettime() - start);
+ diagnostics_.acc_(ros::Time::now().toSec() - start);
// Convert status back to HW Interface
current = current_buffer_;
@@ -333,7 +333,7 @@
--reset_state_;
// Update current time
- hw_->current_time_ = realtime_gettime();
+ hw_->current_time_ = ros::Time::now().toSec();
unsigned char *tmp = current_buffer_;
current_buffer_ = last_buffer_;
Modified: pkg/trunk/stacks/pr2_ethercat_drivers/ethercat_hardware/src/wg0x.cpp
===================================================================
--- pkg/trunk/stacks/pr2_ethercat_drivers/ethercat_hardware/src/wg0x.cpp 2009-08-07 18:26:25 UTC (rev 21033)
+++ pkg/trunk/stacks/pr2_ethercat_drivers/ethercat_hardware/src/wg0x.cpp 2009-08-07 18:38:25 UTC (rev 21034)
@@ -358,7 +358,7 @@
{
if (pressure_publisher_ && pressure_publisher_->trylock())
{
- pressure_publisher_->msg_.header.stamp = ros::Time(realtime_gettime());
+ pressure_publisher_->msg_.header.stamp = ros::Time::now();
pressure_publisher_->msg_.set_data0_size(22);
pressure_publisher_->msg_.set_data1_size(22);
for (int i = 0; i < 22; ++i ) {
Modified: pkg/trunk/util/realtime_tools/CMakeLists.txt
===================================================================
--- pkg/trunk/util/realtime_tools/CMakeLists.txt 2009-08-07 18:26:25 UTC (rev 21033)
+++ pkg/trunk/util/realtime_tools/CMakeLists.txt 2009-08-07 18:38:25 UTC (rev 21034)
@@ -1,8 +1,3 @@
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
rospack(realtime_tools)
-
-rospack_add_boost_directories()
-
-rospack_add_library(realtime_tools src/realtime_tools.cpp)
-rospack_link_boost(realtime_tools thread)
Modified: pkg/trunk/util/realtime_tools/include/realtime_tools/realtime_publisher.h
===================================================================
--- pkg/trunk/util/realtime_tools/include/realtime_tools/realtime_publisher.h 2009-08-07 18:26:25 UTC (rev 21033)
+++ pkg/trunk/util/realtime_tools/include/realtime_tools/realtime_publisher.h 2009-08-07 18:38:25 UTC (rev 21034)
@@ -42,7 +42,6 @@
#include <ros/node_handle.h>
#include <boost/thread/mutex.hpp>
#include <boost/thread/thread.hpp>
-#include <realtime_tools/realtime_tools.h>
namespace realtime_tools {
@@ -53,17 +52,6 @@
void construct(int queue_size)
{
publisher_ = node_.advertise<Msg>(topic_, queue_size);
-
- if (0 != realtime_cond_create(&updated_cond_))
- {
- perror("realtime_cond_create");
- abort();
- }
- if (0 != realtime_mutex_create(&msg_lock_))
- {
- perror("realtime_mutex_create");
- abort();
- }
keep_running_ = true;
thread_ = boost::thread(&RealtimePublisher::publishingLoop, this);
}
@@ -89,28 +77,24 @@
usleep(100);
publisher_.shutdown();
-
- // Destroy thread resources
- realtime_cond_delete(&updated_cond_);
- realtime_mutex_delete(&msg_lock_);
}
void stop()
{
keep_running_ = false;
- realtime_cond_signal(&updated_cond_); // So the publishing loop can exit
+ updated_cond_.notify_one(); // So the publishing loop can exit
}
Msg msg_;
- int lock()
+ void lock()
{
- return realtime_mutex_lock(&msg_lock_);
+ msg_mutex_.lock();
}
bool trylock()
{
- if (0 == realtime_mutex_trylock(&msg_lock_))
+ if (msg_mutex_.try_lock())
{
if (turn_ == REALTIME)
{
@@ -118,7 +102,7 @@
}
else
{
- realtime_mutex_unlock(&msg_lock_);
+ msg_mutex_.unlock();
return false;
}
}
@@ -128,43 +112,39 @@
void unlock()
{
- realtime_mutex_unlock(&msg_lock_);
+ msg_mutex_.unlock();
}
void unlockAndPublish()
{
turn_ = NON_REALTIME;
- realtime_mutex_unlock(&msg_lock_);
- realtime_cond_signal(&updated_cond_);
+ msg_mutex_.unlock();
+ updated_cond_.notify_one();
}
bool is_running() const { return is_running_; }
void publishingLoop()
{
- RealtimeTask task;
-
- int err = realtime_shadow_task(&task);
- if (err)
- ROS_WARN("Unable to shadow task: %d\n", err);
-
is_running_ = true;
turn_ = REALTIME;
while (keep_running_)
{
+ Msg outgoing;
// Locks msg_ and copies it
- realtime_mutex_lock(&msg_lock_);
- while (turn_ != NON_REALTIME)
{
- if (!keep_running_)
- break;
- realtime_cond_wait(&updated_cond_, &msg_lock_);
+ boost::unique_lock<boost::mutex> lock(msg_mutex_);
+ while (turn_ != NON_REALTIME)
+ {
+ if (!keep_running_)
+ break;
+ updated_cond_.wait(lock);
+ }
+
+ outgoing = msg_;
+ turn_ = REALTIME;
}
- Msg outgoing(msg_);
- turn_ = REALTIME;
- realtime_mutex_unlock(&msg_lock_);
-
// Sends the outgoing message
if (keep_running_)
publisher_.publish(outgoing);
@@ -183,8 +163,8 @@
boost::thread thread_;
- RealtimeMutex msg_lock_; // Protects msg_
- RealtimeCond updated_cond_;
+ boost::mutex msg_mutex_; // Protects msg_
+ boost::condition_variable updated_cond_;
enum {REALTIME, NON_REALTIME};
int turn_; // Who's turn is it to use msg_?
Modified: pkg/trunk/util/realtime_tools/include/realtime_tools/realtime_srv_call.h
===================================================================
--- pkg/trunk/util/realtime_tools/include/realtime_tools/realtime_srv_call.h 2009-08-07 18:26:25 UTC (rev 21033)
+++ pkg/trunk/util/realtime_tools/include/realtime_tools/realtime_srv_call.h 2009-08-07 18:38:25 UTC (rev 21034)
@@ -43,7 +43,6 @@
#include <ros/ros.h>
#include <boost/thread/mutex.hpp>
#include <boost/thread/thread.hpp>
-#include <realtime_tools/realtime_tools.h>
namespace realtime_tools {
@@ -56,17 +55,6 @@
{
client_ = node_.serviceClient<SrvReq, SrvRes>(topic);
- if (0 != realtime_cond_create(&updated_cond_))
- {
- perror("realtime_cond_create");
- abort();
- }
- if (0 != realtime_mutex_create(&srv_lock_))
- {
- perror("realtime_mutex_create");
- abort();
- }
-
// Makes the trylock() fail until the service is ready
lock();
ROS_INFO("RealtimeSrvCall is waiting for %s", topic.c_str() );
@@ -85,29 +73,25 @@
usleep(100);
client_.shutdown();
-
- // Destroy thread resources
- realtime_cond_delete(&updated_cond_);
- realtime_mutex_delete(&srv_lock_);
}
void stop()
{
keep_running_ = false;
- realtime_cond_signal(&updated_cond_); // So the call loop can exit
+ updated_cond_.notify_one(); // So the call loop can exit
}
SrvReq srv_req_;
SrvRes srv_res_;
- int lock()
+ void lock()
{
- return realtime_mutex_lock(&srv_lock_);
+ srv_mutex_.lock();
}
bool trylock()
{
- if (0 == realtime_mutex_trylock(&srv_lock_))
+ if (srv_mutex_.try_lock())
{
if (turn_ == REALTIME)
{
@@ -115,7 +99,7 @@
}
else
{
- realtime_mutex_unlock(&srv_lock_);
+ srv_mutex_.unlock();
return false;
}
}
@@ -125,43 +109,40 @@
void unlock()
{
- realtime_mutex_unlock(&srv_lock_);
+ srv_mutex_.unlock();
}
void unlockAndCall()
{
turn_ = NON_REALTIME;
- realtime_mutex_unlock(&srv_lock_);
- realtime_cond_signal(&updated_cond_);
+ srv_mutex_.unlock();
+ updated_cond_.notify_one();
}
bool is_running() const { return is_running_; }
void callLoop()
{
- RealtimeTask task;
-
- int err = realtime_shadow_task(&task);
- if (err)
- ROS_WARN("Unable to shadow task: %d\n", err);
-
is_running_ = true;
turn_ = REALTIME;
while (keep_running_)
{
+ SrvReq outgoing;
+ SrvRes incoming;
// Locks Srv_ and copies it
- realtime_mutex_lock(&srv_lock_);
- while (turn_ != NON_REALTIME)
{
- if (!keep_running_)
- break;
- realtime_cond_wait(&updated_cond_, &srv_lock_);
- }
+ boost::unique_lock<boost::mutex> lock(srv_mutex_);
+ while (turn_ != NON_REALTIME)
+ {
+ if (!keep_running_)
+ break;
+ updated_cond_.wait(lock);
+ }
- SrvReq outgoing(srv_req_);
- SrvRes incoming(srv_res_);
- turn_ = REALTIME;
- realtime_mutex_unlock(&srv_lock_);
+ outgoing = srv_req_;
+ incoming = srv_res_;
+ turn_ = REALTIME;
+ }
// Sends the outgoing message
if (keep_running_)
@@ -181,8 +162,8 @@
boost::thread thread_;
- RealtimeMutex srv_lock_; // Protects srv_
- RealtimeCond updated_cond_;
+ boost::mutex srv_mutex_; // Protects srv_
+ boost::condition_variable updated_cond_;
enum {REALTIME, NON_REALTIME};
int turn_; // Who's turn is it to use srv_?
Deleted: pkg/trunk/util/realtime_tools/include/realtime_tools/realtime_tools.h
===================================================================
--- pkg/trunk/util/realtime_tools/include/realtime_tools/realtime_tools.h 2009-08-07 18:26:25 UTC (rev 21033)
+++ pkg/trunk/util/realtime_tools/include/realtime_tools/realtime_tools.h 2009-08-07 18:38:25 UTC (rev 21034)
@@ -1,79 +0,0 @@
-/*
- * Copyright (c) 2008, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in the
- * documentation and/or other materials provided with the distribution.
- * * Neither the name of the Willow Garage, Inc. nor the names of its
- * contributors may be used to endorse or promote products derived from
- * this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-#ifndef REALTIME_TOOLS_H
-#define REALTIME_TOOLS_H
-
-#include <pthread.h>
-#include <ros/time.h>
-
-typedef unsigned long xnhandle_t;
-typedef struct {
- xnhandle_t opaque;
-} XN_RT_MUTEX;
-
-typedef struct {
- xnhandle_t opaque;
-} XN_RT_COND;
-
-typedef struct {
- xnhandle_t opaque;
- unsigned long opaque2;
-} XN_RT_TASK;
-
-typedef union {
- XN_RT_MUTEX rt;
- pthread_mutex_t pt;
-} RealtimeMutex;
-
-typedef union {
- XN_RT_COND rt;
- pthread_cond_t pt;
-} RealtimeCond;
-
-typedef union {
- XN_RT_TASK rt;
-} RealtimeTask;
-
-int realtime_mutex_create(RealtimeMutex *mutex);
-int realtime_mutex_delete(RealtimeMutex *mutex);
-int realtime_mutex_lock(RealtimeMutex *mutex);
-int realtime_mutex_trylock(RealtimeMutex *mutex);
-int realtime_mutex_unlock(RealtimeMutex *mutex);
-
-int realtime_cond_create(RealtimeCond *cond);
-int realtime_cond_delete(RealtimeCond *cond);
-int realtime_cond_signal(RealtimeCond *cond);
-int realtime_cond_wait(RealtimeCond *cond, RealtimeMutex *mutex);
-
-int realtime_shadow_task(RealtimeTask *task);
-
-double realtime_gettime(void);
-
-#endif
Deleted: pkg/trunk/util/realtime_tools/include/realtime_tools/xenomai_prototypes.h
===================================================================
--- pkg/trunk/util/realtime_tools/include/realtime_tools/xenomai_prototypes.h 2009-08-07 18:26:25 UTC (rev 21033)
+++ pkg/trunk/util/realtime_tools/include/realtime_tools/xenomai_prototypes.h 2009-08-07 18:38:25 UTC (rev 21034)
@@ -1,80 +0,0 @@
-/*
- * Copyright (c) 2009, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in the
- * documentation and/or other materials provided with the distribution.
- * * Neither the name of the Willow Garage, Inc. nor the names of its
- * contributors may be used to endorse or promote products derived from
- * this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-#ifndef XENOMAI_PROTOTYPES_H
-#define XENOMAI_PROTOTYPES_H
-
-#include "realtime_tools/realtime_tools.h"
-
-typedef unsigned long long xnticks_t;
-typedef xnticks_t RTIME;
-
-#define TM_INFINITE (0)
-#define TM_NONBLOCK ((xnticks_t)-1)
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-/* Mutex Interface */
-int rt_mutex_create(XN_RT_MUTEX *mutex, const char *name) __attribute__((weak));
-
-int rt_mutex_delete(XN_RT_MUTEX *mutex) __attribute__((weak));
-
-int rt_mutex_acquire(XN_RT_MUTEX *mutex, RTIME timeout) __attribute__((weak));
-
-int rt_mutex_release(XN_RT_MUTEX *mutex) __attribute__((weak));
-
-/* Condition Variable Interface */
-int rt_cond_create(XN_RT_COND *cond, const char *name) __attribute__((weak));
-
-int rt_cond_delete(XN_RT_COND *cond) __attribute__((weak));
-
-int rt_cond_signal(XN_RT_COND *cond) __attribute__((weak));
-
-int rt_cond_wait(XN_RT_COND *cond,
- XN_RT_MUTEX *mutex,
- RTIME timeout) __attribute__((weak));
-
-/* Task Interface */
-int rt_task_shadow(XN_RT_TASK *task,
- const char *name,
- int prio,
- int mode) __attribute__((weak));
-
-/* Timer Interface */
-RTIME rt_timer_read(void) __attribute__((weak));
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif
-
-
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
