Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[4740] pkg/trunk
|
From: <hsu...@us...> - 2008-09-27 00:57:26
|
Revision: 4740
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4740&view=rev
Author: hsujohnhsu
Date: 2008-09-27 00:57:16 +0000 (Sat, 27 Sep 2008)
Log Message:
-----------
fixing proper shutdown behavior. unsubscribe, unadvertise.
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_position_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_position_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_position_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_position_controller.cpp
pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_position_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_position_controller.h 2008-09-27 00:55:04 UTC (rev 4739)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_position_controller.h 2008-09-27 00:57:16 UTC (rev 4740)
@@ -280,11 +280,22 @@
private:
pr2_mechanism_controllers::JointPosCmd msg_; //The message used by the ROS callback
ArmPositionController *c_;
+
+ /*!
+ * \brief service prefix
+ */
+ std::string service_prefix_;
+
/*
* \brief save topic name for unsubscribe later
*/
const char * topic_name_;
+ /*!
+ * \brief xml pointer to ros topic name
+ */
+ TiXmlElement * ros_cb_;
+
/*
* \brief pointer to ros node
*/
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_controller.h 2008-09-27 00:55:04 UTC (rev 4739)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_controller.h 2008-09-27 00:57:16 UTC (rev 4740)
@@ -234,6 +234,16 @@
private:
LaserScannerController *c_;
+ /*!
+ * \brief service prefix
+ */
+ std::string service_prefix_;
+
+ /*!
+ * \brief A pointer to ros node
+ */
+ ros::node *node_;
+
};
}
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_position_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_position_controller.cpp 2008-09-27 00:55:04 UTC (rev 4739)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_position_controller.cpp 2008-09-27 00:57:16 UTC (rev 4740)
@@ -230,7 +230,15 @@
ArmPositionControllerNode::~ArmPositionControllerNode()
{
- node_->unsubscribe(topic_name_);
+ node_->unadvertise_service(service_prefix_ + "/set_command");
+ node_->unadvertise_service(service_prefix_ + "/set_command_array");
+ node_->unadvertise_service(service_prefix_ + "/get_command");
+ node_->unadvertise_service(service_prefix_ + "/set_target");
+
+ std::cout << "unsub arm controller" << ros_cb_ << " " << topic_name_ << std::endl;
+ if(ros_cb_ && topic_name_)
+ node_->unsubscribe(topic_name_);
+
delete c_;
}
@@ -242,20 +250,20 @@
bool ArmPositionControllerNode::initXml(mechanism::RobotState * robot, TiXmlElement * config)
{
std::cout<<"LOADING ARMCONTROLLERNODE"<<std::endl;
- string prefix = config->Attribute("name");
- std::cout<<"the prefix is "<<prefix<<std::endl;
+ service_prefix_ = config->Attribute("name");
+ std::cout<<"the service_prefix_ is "<<service_prefix_<<std::endl;
// Parses subcontroller configuration
if(c_->initXml(robot, config))
{
- node_->advertise_service(prefix + "/set_command", &ArmPositionControllerNode::setJointPosHeadless, this);
- node_->advertise_service(prefix + "/set_command_array", &ArmPositionControllerNode::setJointPosSrv, this);
- node_->advertise_service(prefix + "/get_command", &ArmPositionControllerNode::getJointPosCmd, this);
- node_->advertise_service(prefix + "/set_target", &ArmPositionControllerNode::setJointPosTarget, this);
+ node_->advertise_service(service_prefix_ + "/set_command", &ArmPositionControllerNode::setJointPosHeadless, this);
+ node_->advertise_service(service_prefix_ + "/set_command_array", &ArmPositionControllerNode::setJointPosSrv, this);
+ node_->advertise_service(service_prefix_ + "/get_command", &ArmPositionControllerNode::getJointPosCmd, this);
+ node_->advertise_service(service_prefix_ + "/set_target", &ArmPositionControllerNode::setJointPosTarget, this);
- TiXmlElement * ros_cb = config->FirstChildElement("listen_topic");
- if(ros_cb)
+ ros_cb_ = config->FirstChildElement("listen_topic");
+ if(ros_cb_)
{
- topic_name_=ros_cb->Attribute("name");
+ topic_name_=ros_cb_->Attribute("name");
if(!topic_name_)
{
std::cout<<" A listen _topic is present in the xml file but no name is specified\n";
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_controller.cpp 2008-09-27 00:55:04 UTC (rev 4739)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_controller.cpp 2008-09-27 00:57:16 UTC (rev 4740)
@@ -392,7 +392,7 @@
}
ROS_REGISTER_CONTROLLER(LaserScannerControllerNode)
-LaserScannerControllerNode::LaserScannerControllerNode()
+LaserScannerControllerNode::LaserScannerControllerNode(): node_(ros::node::instance())
{
c_ = new LaserScannerController();
}
@@ -400,6 +400,9 @@
LaserScannerControllerNode::~LaserScannerControllerNode()
{
+ node_->unadvertise_service(service_prefix_ + "/set_command");
+ node_->unadvertise_service(service_prefix_ + "/get_command");
+ node_->unadvertise_service(service_prefix_ + "/set_profile");
delete c_;
}
@@ -458,14 +461,13 @@
bool LaserScannerControllerNode::initXml(mechanism::RobotState *robot, TiXmlElement *config)
{
- ros::node *node = ros::node::instance();
- string prefix = config->Attribute("name");
+ service_prefix_ = config->Attribute("name");
if (!c_->initXml(robot, config))
return false;
- node->advertise_service(prefix + "/set_command", &LaserScannerControllerNode::setCommand, this);
- node->advertise_service(prefix + "/get_command", &LaserScannerControllerNode::getCommand, this);
- node->advertise_service(prefix + "/set_profile", &LaserScannerControllerNode::setProfileCall, this);
+ node_->advertise_service(service_prefix_ + "/set_command", &LaserScannerControllerNode::setCommand, this);
+ node_->advertise_service(service_prefix_ + "/get_command", &LaserScannerControllerNode::getCommand, this);
+ node_->advertise_service(service_prefix_ + "/set_profile", &LaserScannerControllerNode::setProfileCall, this);
return true;
}
bool LaserScannerControllerNode::getActual(
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 2008-09-27 00:55:04 UTC (rev 4739)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_position_controller.h 2008-09-27 00:57:16 UTC (rev 4740)
@@ -201,6 +201,11 @@
const char * topic_name_;
/*!
+ * \brief xml pointer to ros topic name
+ */
+ TiXmlElement * ros_cb;
+
+ /*!
* \brief A pointer to ros node
*/
ros::node *node_;
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/joint_position_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/joint_position_controller.cpp 2008-09-27 00:55:04 UTC (rev 4739)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/joint_position_controller.cpp 2008-09-27 00:57:16 UTC (rev 4740)
@@ -167,7 +167,8 @@
{
node_->unadvertise_service(service_prefix_ + "/set_command");
node_->unadvertise_service(service_prefix_ + "/get_actual");
- node_->unsubscribe(topic_name_);
+ if(ros_cb && topic_name_)
+ node_->unsubscribe(topic_name_);
delete c_;
}
@@ -233,7 +234,7 @@
node_->advertise_service(service_prefix_ + "/get_actual", &JointPositionControllerNode::getActual, this);
guard_get_actual_.set(service_prefix_ + "/get_actual");
- TiXmlElement * ros_cb = config->FirstChildElement("listen_topic");
+ ros_cb = config->FirstChildElement("listen_topic");
if(ros_cb)
{
topic_name_=ros_cb->Attribute("name");
Modified: pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-09-27 00:55:04 UTC (rev 4739)
+++ pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-09-27 00:57:16 UTC (rev 4740)
@@ -44,11 +44,11 @@
MechanismControl::~MechanismControl()
{
// Destroy all controllers
- // for (int i = 0; i < MAX_NUM_CONTROLLERS; ++i)
- // {
- // if (controllers_[i] != NULL)
- // delete controllers_[i];
- // }
+ for (int i = 0; i < MAX_NUM_CONTROLLERS; ++i)
+ {
+ if (controllers_[i] != NULL)
+ delete controllers_[i];
+ }
if (state_)
delete state_;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
