Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[17603] pkg/trunk
|
From: <hsu...@us...> - 2009-06-24 19:19:23
|
Revision: 17603
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17603&view=rev
Author: hsujohnhsu
Date: 2009-06-24 19:19:18 +0000 (Wed, 24 Jun 2009)
Log Message:
-----------
update to gazebo svn revision 7888
gazebo plugins now using node handles api
Revision Links:
--------------
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7888&view=rev
Modified Paths:
--------------
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_battery.h
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_mechanism_control.h
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_block_laser.h
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_bumper.h
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_camera.h
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_f3d.h
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_laser.h
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_p3d.h
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_prosilica.h
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_ptz.h
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_sim_iface.h
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_stereo_camera.h
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_time.h
pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_battery.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_mechanism_control.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_block_laser.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_bumper.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_camera.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_f3d.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_laser.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_p3d.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_prosilica.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_ptz.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_sim_iface.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_stereo_camera.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_time.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2factory.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/xml2factory.cpp
pkg/trunk/stacks/simulators/gazebo/Makefile
Added Paths:
-----------
pkg/trunk/stacks/simulators/gazebo/gazebo_new_patch.diff
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_battery.h
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_battery.h 2009-06-24 18:00:57 UTC (rev 17602)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_battery.h 2009-06-24 19:19:18 UTC (rev 17603)
@@ -39,7 +39,7 @@
#include <diagnostic_msgs/DiagnosticMessage.h>
#include <diagnostic_msgs/DiagnosticStatus.h>
#include <gazebo_plugin/PlugCommand.h>
-#include <ros/node.h>
+#include <ros/ros.h>
#include <boost/thread/mutex.hpp>
namespace gazebo
@@ -115,7 +115,9 @@
diagnostic_msgs::DiagnosticStatus diagnostic_status_;
/// \brief pointer to ros node
- private: ros::Node *rosnode_;
+ private: ros::NodeHandle* rosnode_;
+ private: ros::Publisher pub_;
+ private: ros::Subscriber sub_;
/// \brief battery state topic name
private: std::string stateTopicName_;
@@ -155,8 +157,7 @@
private: double default_consumption_rate_;
/// \brief listen to ROS to see if we are charging
- private: void SetPlug();
- private: gazebo_plugin::PlugCommand plug_msg_;
+ private: void SetPlug(const gazebo_plugin::PlugCommandConstPtr& plug_msg);
};
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_mechanism_control.h
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_mechanism_control.h 2009-06-24 18:00:57 UTC (rev 17602)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_mechanism_control.h 2009-06-24 19:19:18 UTC (rev 17603)
@@ -157,7 +157,7 @@
/*
* \brief pointer to ros node
*/
- ros::Node *rosnode_;
+ ros::NodeHandle* rosnode_;
/*
* \brief tmp vars for performance checking
@@ -165,8 +165,8 @@
double wall_start, sim_start;
/// \brief set topic name of robotdesc parameter
- private: ParamT<std::string> *robotParamP;
- private: std::string robotParam;
+ ParamT<std::string> *robotParamP;
+ std::string robotParam;
};
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_block_laser.h
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_block_laser.h 2009-06-24 18:00:57 UTC (rev 17602)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_block_laser.h 2009-06-24 19:19:18 UTC (rev 17603)
@@ -30,7 +30,7 @@
#include <gazebo/Controller.hh>
-#include <ros/node.h>
+#include <ros/ros.h>
#include "boost/thread/mutex.hpp"
#include <robot_msgs/PointCloud.h>
@@ -159,7 +159,8 @@
private: RaySensor *myParent;
/// \brief pointer to ros node
- private: ros::Node *rosnode;
+ private: ros::NodeHandle* rosnode_;
+ private: ros::Publisher pub_;
/// \brief ros message
private: robot_msgs::PointCloud cloudMsg;
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_bumper.h
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_bumper.h 2009-06-24 18:00:57 UTC (rev 17602)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_bumper.h 2009-06-24 19:19:18 UTC (rev 17603)
@@ -33,7 +33,7 @@
#include <gazebo/Param.hh>
// ros messages
-#include <ros/node.h>
+#include <ros/ros.h>
#include "boost/thread/mutex.hpp"
#include <std_msgs/String.h>
#include <robot_msgs/Vector3Stamped.h>
@@ -96,7 +96,8 @@
/// \brief pointer to ros node
- private: ros::Node *rosnode;
+ private: ros::NodeHandle* rosnode_;
+ private: ros::Publisher info_pub_,force_pub_;
/// \brief set topic name of broadcast
private: ParamT<std::string> *bumperTopicNameP;
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_camera.h
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_camera.h 2009-06-24 18:00:57 UTC (rev 17602)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_camera.h 2009-06-24 19:19:18 UTC (rev 17603)
@@ -27,7 +27,7 @@
#ifndef ROS_CAMERA_HH
#define ROS_CAMERA_HH
-#include <ros/node.h>
+#include <ros/ros.h>
#include "boost/thread/mutex.hpp"
#include <image_msgs/Image.h>
#include <gazebo/Param.hh>
@@ -118,7 +118,8 @@
private: MonoCameraSensor *myParent;
/// \brief A pointer to the ROS node. A node will be instantiated if it does not exist.
- private: ros::Node *rosnode;
+ private: ros::NodeHandle* rosnode_;
+ private: ros::Publisher pub_;
/// \brief ROS image message
private: image_msgs::Image imageMsg;
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_f3d.h
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_f3d.h 2009-06-24 18:00:57 UTC (rev 17602)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_f3d.h 2009-06-24 19:19:18 UTC (rev 17603)
@@ -32,7 +32,7 @@
#include <gazebo/Model.hh>
#include <gazebo/Body.hh>
-#include <ros/node.h>
+#include <ros/ros.h>
#include <boost/thread/mutex.hpp>
#include <robot_msgs/Vector3Stamped.h>
@@ -95,7 +95,8 @@
/// \brief A pointer to the ROS node. A node will be instantiated if it does not exist.
- private: ros::Node *rosnode;
+ private: ros::NodeHandle* rosnode_;
+ private: ros::Publisher pub_;
/// \brief ROS Vector3Stamped message
private: robot_msgs::Vector3Stamped vector3Msg;
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_laser.h
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_laser.h 2009-06-24 18:00:57 UTC (rev 17602)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_laser.h 2009-06-24 19:19:18 UTC (rev 17603)
@@ -30,7 +30,7 @@
#include <gazebo/Controller.hh>
-#include <ros/node.h>
+#include <ros/ros.h>
#include <boost/thread/mutex.hpp>
#include <laser_scan/LaserScan.h>
@@ -141,7 +141,8 @@
private: RaySensor *myParent;
/// \brief pointer to ros node
- private: ros::Node *rosnode;
+ private: ros::NodeHandle* rosnode_;
+ private: ros::Publisher pub_;
/// \brief ros message
private: laser_scan::LaserScan laserMsg;
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_p3d.h
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_p3d.h 2009-06-24 18:00:57 UTC (rev 17602)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_p3d.h 2009-06-24 19:19:18 UTC (rev 17603)
@@ -32,7 +32,7 @@
#include <gazebo/Model.hh>
#include <gazebo/Body.hh>
-#include <ros/node.h>
+#include <ros/ros.h>
#include "boost/thread/mutex.hpp"
#include <robot_msgs/PoseWithRatesStamped.h>
@@ -118,7 +118,8 @@
/// \brief pointer to ros node
- private: ros::Node *rosnode;
+ private: ros::NodeHandle* rosnode_;
+ private: ros::Publisher pub_;
/// \brief ros message
private: robot_msgs::PoseWithRatesStamped poseMsg;
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_prosilica.h
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_prosilica.h 2009-06-24 18:00:57 UTC (rev 17602)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_prosilica.h 2009-06-24 19:19:18 UTC (rev 17603)
@@ -27,7 +27,7 @@
#ifndef ROS_CAMERA_HH
#define ROS_CAMERA_HH
-#include <ros/node.h>
+#include <ros/ros.h>
#include "boost/thread/mutex.hpp"
#include <gazebo/Param.hh>
#include <gazebo/Controller.hh>
@@ -137,7 +137,9 @@
private: MonoCameraSensor *myParent;
/// \brief A pointer to the ROS node. A node will be instantiated if it does not exist.
- private: ros::Node *rosnode;
+ private: ros::NodeHandle* rosnode_;
+ private: ros::Publisher image_pub_,image_rect_pub_,cam_info_pub_;
+ private: ros::ServiceServer cam_info_ser_,poll_ser_;
/// \brief ros message
/// \brief construct raw stereo message
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_ptz.h
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_ptz.h 2009-06-24 18:00:57 UTC (rev 17602)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_ptz.h 2009-06-24 19:19:18 UTC (rev 17603)
@@ -33,7 +33,7 @@
#include <gazebo/Model.hh>
// ros messages
-#include <ros/node.h>
+#include <ros/ros.h>
#include "boost/thread/mutex.hpp"
// messages for controlling ptz
@@ -146,13 +146,15 @@
private: ParamT<std::string> *stateTopicNameP;
/// \brief pointer to ros node
- private: ros::Node *rosnode;
+ private: ros::NodeHandle* rosnode_;
+ private: ros::Publisher pub_;
+ private: ros::Subscriber sub_;
+
/// \brief ros message
private: axis_cam::PTZActuatorState PTZStateMessage;
- private: axis_cam::PTZActuatorCmd PTZControlMessage;
/// \brief receive message
- private: void PTZCommandReceived();
+ private: void PTZCommandReceived(const axis_cam::PTZActuatorCmdConstPtr& in);
/// \brief topic name
private: std::string commandTopicName;
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_sim_iface.h
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_sim_iface.h 2009-06-24 18:00:57 UTC (rev 17602)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_sim_iface.h 2009-06-24 19:19:18 UTC (rev 17603)
@@ -27,7 +27,7 @@
#ifndef ROS_SIM_IFACE_HH
#define ROS_SIM_IFACE_HH
-#include <ros/node.h>
+#include <ros/ros.h>
#include "boost/thread/mutex.hpp"
#include <gazebo/Controller.hh>
#include <gazebo/Param.hh>
@@ -91,17 +91,15 @@
protected: virtual void FiniChild();
/// \brief call back when a PoseWithRatesStamped message is published
- private: void UpdateObjectPose();
+ private: void UpdateObjectPose(const robot_msgs::PoseWithRatesStampedConstPtr& poseMsg);
/// \brief A pointer to the parent entity
private: Entity *myParent;
/// \brief A pointer to the ROS node. A node will be instantiated if it does not exist.
- private: ros::Node *rosnode;
+ private: ros::NodeHandle* rosnode_;
+ private: ros::Subscriber sub_;
- /// \brief ROS PoseWithRatesStamped message
- private: robot_msgs::PoseWithRatesStamped poseMsg;
-
/// \brief A mutex to lock access to fields that are used in ROS message callbacks
private: boost::mutex lock;
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_stereo_camera.h
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_stereo_camera.h 2009-06-24 18:00:57 UTC (rev 17602)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_stereo_camera.h 2009-06-24 19:19:18 UTC (rev 17603)
@@ -28,7 +28,7 @@
#ifndef ROS_STEREO_CAMERA_HH
#define ROS_STEREO_CAMERA_HH
-#include <ros/node.h>
+#include <ros/ros.h>
#include "boost/thread/mutex.hpp"
#include <gazebo/Generic_Camera.hh>
@@ -159,7 +159,8 @@
private: double baseline;
/// \brief pointer to ros node
- private: ros::Node *rosnode;
+ private: ros::NodeHandle* rosnode_;
+ private: ros::Publisher pub_;
/// \brief ros message
/// \brief construct raw stereo message
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_time.h
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_time.h 2009-06-24 18:00:57 UTC (rev 17602)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_time.h 2009-06-24 19:19:18 UTC (rev 17603)
@@ -31,7 +31,7 @@
#include <gazebo/Body.hh>
#include <gazebo/World.hh>
-#include <ros/node.h>
+#include <ros/ros.h>
#include "boost/thread/mutex.hpp"
// roscpp - used for broadcasting time over ros
@@ -121,8 +121,11 @@
/// \brief A mutex to lock access to fields that are used in message callbacks
private: boost::mutex lock;
+
/// \brief pointer to ros node
- ros::Node *rosnode_;
+ private: ros::NodeHandle* rosnode_;
+ private: ros::Publisher pub_;
+
roslib::Time timeMsg;
};
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_battery.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_battery.cpp 2009-06-24 18:00:57 UTC (rev 17602)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_battery.cpp 2009-06-24 19:19:18 UTC (rev 17603)
@@ -55,31 +55,26 @@
if (!this->parent_model_)
gzthrow("GazeboBattery controller requires a Model as its parent");
- rosnode_ = ros::g_node; // comes from where?
- int argc = 0;
- char** argv = NULL;
- if (rosnode_ == NULL)
- {
- // this only works for a single camera.
- ros::init(argc,argv);
- rosnode_ = new ros::Node("ros_gazebo",ros::Node::DONT_HANDLE_SIGINT);
- ROS_DEBUG("Starting node in P3D");
- }
+ int argc = 0;
+ char** argv = NULL;
+ ros::init(argc,argv,"gazebo_battery");
+ this->rosnode_ = new ros::NodeHandle();
}
GazeboBattery::~GazeboBattery()
{
+ delete this->rosnode_;
}
void GazeboBattery::LoadChild(XMLConfigNode *node)
{
this->stateTopicName_ = node->GetString("stateTopicName","battery_state",0);
- rosnode_->advertise<robot_msgs::BatteryState>(this->stateTopicName_,10);
+ this->pub_ = this->rosnode_->advertise<robot_msgs::BatteryState>(this->stateTopicName_,10);
//this->diagnosticMessageTopicName_ = node->GetString("diagnosticMessageTopicName","diagnostic",0);
- //rosnode_->advertise<diagnostic_msgs::DiagnosticMessage>(this->diagnosticMessageTopicName_,10);
+ //this->diag_pub_ = this->rosnode_->advertise<diagnostic_msgs::DiagnosticMessage>(this->diagnosticMessageTopicName_,10);
/// faking the plug and unplug of robot
- rosnode_->subscribe("plugged_in",this->plug_msg_,&GazeboBattery::SetPlug,this,10);
+ this->sub_ = this->rosnode_->subscribe("plugged_in",10,&GazeboBattery::SetPlug,this);
this->default_consumption_rate_ = node->GetDouble("default_consumption_rate",-10.0,0);
this->full_capacity_ = node->GetDouble("full_charge_energy",0.0,0);
@@ -91,10 +86,10 @@
this->battery_state_rate_ = node->GetDouble("dbattery_state_rate_",1.0,0);
}
- void GazeboBattery::SetPlug()
+ void GazeboBattery::SetPlug(const gazebo_plugin::PlugCommandConstPtr& plug_msg)
{
this->lock_.lock();
- if (this->plug_msg_.status == "the robot is very much plugged into the wall")
+ if (plug_msg->status == "the robot is very much plugged into the wall")
this->consumption_rate_ = this->default_charge_rate_ + this->default_consumption_rate_;
else
this->consumption_rate_ = this->default_consumption_rate_;
@@ -138,7 +133,7 @@
this->battery_state_.power_consumption = this->consumption_rate_;
this->lock_.lock();
- this->rosnode_->publish(this->stateTopicName_,this->battery_state_);
+ this->pub_.publish(this->battery_state_);
this->lock_.unlock();
/**********************************************************/
@@ -153,7 +148,7 @@
//this->diagnostic_message_.set_status_size(1);
//this->diagnostic_message_.status[0] = this->diagnostic_status_;
//this->lock_.lock();
- //this->rosnode_->publish(this->diagnosticMessageTopicName_,diagnostic_message_);
+ //this->diag_pub_.publish(this->diagnosticMessageTopicName_,diagnostic_message_);
//this->lock_.unlock();
this->last_time_ = this->current_time_;
@@ -162,11 +157,6 @@
void GazeboBattery::FiniChild()
{
ROS_DEBUG("Calling FiniChild in GazeboBattery");
-
- rosnode_->unadvertise(this->stateTopicName_);
- //rosnode_->unadvertise(this->diagnosticMessageTopicName_);
- rosnode_->unsubscribe("plugged_in");
-
}
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_mechanism_control.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_mechanism_control.cpp 2009-06-24 18:00:57 UTC (rev 17602)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_mechanism_control.cpp 2009-06-24 19:19:18 UTC (rev 17603)
@@ -61,28 +61,22 @@
this->robotParamP = new ParamT<std::string>("robotParam", "robotdesc/pr2", 0);
Param::End();
- rosnode_ = ros::g_node; // comes from where?
- int argc = 0;
- char** argv = NULL;
- if (rosnode_ == NULL)
- {
- // this only works for a single camera.
- ros::init(argc,argv);
- rosnode_ = new ros::Node("ros_gazebo",ros::Node::DONT_HANDLE_SIGINT);
- ROS_DEBUG("Starting node in Gazebo Mechanism Control");
- }
-
if (getenv("CHECK_SPEEDUP"))
{
wall_start = Simulator::Instance()->GetWallTime();
sim_start = Simulator::Instance()->GetSimTime();
}
+ int argc = 0;
+ char** argv = NULL;
+ ros::init(argc,argv,"gazebo_mechanism_control");
+ this->rosnode_ = new ros::NodeHandle();
}
GazeboMechanismControl::~GazeboMechanismControl()
{
delete this->robotParamP;
+ delete this->rosnode_;
}
void GazeboMechanismControl::LoadChild(XMLConfigNode *node)
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_block_laser.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_block_laser.cpp 2009-06-24 18:00:57 UTC (rev 17602)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_block_laser.cpp 2009-06-24 19:19:18 UTC (rev 17603)
@@ -57,23 +57,17 @@
// set parent sensor to active automatically
this->myParent->SetActive(true);
-
- rosnode = ros::g_node; // comes from where? common.h exports as global variable
int argc = 0;
char** argv = NULL;
- if (rosnode == NULL)
- {
- // start a ros node if none exist
- ros::init(argc,argv);
- rosnode = new ros::Node("ros_gazebo",ros::Node::DONT_HANDLE_SIGINT);
- ROS_DEBUG("Starting node in laser");
- }
+ ros::init(argc,argv,"ros_block_laser");
+ this->rosnode_ = new ros::NodeHandle();
}
////////////////////////////////////////////////////////////////////////////////
// Destructor
RosBlockLaser::~RosBlockLaser()
{
+ delete this->rosnode_;
}
////////////////////////////////////////////////////////////////////////////////
@@ -82,7 +76,7 @@
{
this->topicName = node->GetString("topicName","default_ros_laser",0); //read from xml file
ROS_DEBUG("================= %s", this->topicName.c_str());
- rosnode->advertise<robot_msgs::PointCloud>(this->topicName,10);
+ this->pub_ = this->rosnode_->advertise<robot_msgs::PointCloud>(this->topicName,10);
this->frameName = node->GetString("frameName","default_ros_laser",0); //read from xml file
this->gaussianNoise = node->GetDouble("gaussianNoise",0.0,0); //read from xml file
@@ -120,7 +114,6 @@
// Finalize the controller
void RosBlockLaser::FiniChild()
{
- rosnode->unadvertise(this->topicName);
}
////////////////////////////////////////////////////////////////////////////////
@@ -244,7 +237,7 @@
}
// send data out via ros message
- rosnode->publish(this->topicName,this->cloudMsg);
+ this->pub_.publish(this->cloudMsg);
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_bumper.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_bumper.cpp 2009-06-24 18:00:57 UTC (rev 17602)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_bumper.cpp 2009-06-24 19:19:18 UTC (rev 17603)
@@ -35,6 +35,7 @@
#include <gazebo/ControllerFactory.hh>
#include <gazebo/Simulator.hh>
#include <gazebo/Body.hh>
+#include <gazebo/Body.hh>
using namespace gazebo;
@@ -54,17 +55,10 @@
this->bumperTopicNameP = new ParamT<std::string>("bumperTopicName", "bumper", 0);
Param::End();
- rosnode = ros::g_node; // comes from where?
int argc = 0;
char** argv = NULL;
- if (rosnode == NULL)
- {
- // this only works for a single camera.
- ros::init(argc,argv);
- rosnode = new ros::Node("ros_gazebo",ros::Node::DONT_HANDLE_SIGINT);
- ROS_INFO("Starting node in bumper");
- }
-
+ ros::init(argc,argv,"ros_bumper");
+ this->rosnode_ = new ros::NodeHandle();
}
////////////////////////////////////////////////////////////////////////////////
@@ -72,6 +66,7 @@
RosBumper::~RosBumper()
{
delete this->bumperTopicNameP;
+ delete this->rosnode_;
}
////////////////////////////////////////////////////////////////////////////////
@@ -84,8 +79,8 @@
this->bumperTopicName.c_str());
//std::cout << " publishing contact/collisions to topic name: " << this->bumperTopicName << std::endl;
- rosnode->advertise<std_msgs::String>(this->bumperTopicName+std::string("/info"),100);
- rosnode->advertise<robot_msgs::Vector3Stamped>(this->bumperTopicName+std::string("/force"),100);
+ this->info_pub_ = this->rosnode_->advertise<std_msgs::String>(this->bumperTopicName+std::string("/info"),100);
+ this->force_pub_ = this->rosnode_->advertise<robot_msgs::Vector3Stamped>(this->bumperTopicName+std::string("/force"),100);
}
////////////////////////////////////////////////////////////////////////////////
@@ -145,8 +140,8 @@
this->forceMsg.vector.y = (1.0-eps)*this->forceMsg.vector.y + eps*contact_forces.f1[1];
this->forceMsg.vector.z = (1.0-eps)*this->forceMsg.vector.z + eps*contact_forces.f1[2];
- rosnode->publish(this->bumperTopicName+std::string("/info"),this->bumperMsg);
- rosnode->publish(this->bumperTopicName+std::string("/force"),this->forceMsg);
+ this->info_pub_.publish(this->bumperMsg);
+ this->force_pub_.publish(this->forceMsg);
}
}
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_camera.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_camera.cpp 2009-06-24 18:00:57 UTC (rev 17602)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_camera.cpp 2009-06-24 19:19:18 UTC (rev 17603)
@@ -65,22 +65,17 @@
this->frameNameP = new ParamT<std::string>("frameName","stereo_link", 0);
Param::End();
- rosnode = ros::g_node; //@todo: change to handles
int argc = 0;
char** argv = NULL;
- if (rosnode == NULL)
- {
- ros::init(argc,argv);
- rosnode = new ros::Node("ros_gazebo",ros::Node::DONT_HANDLE_SIGINT);
- ROS_DEBUG("Starting node in camera");
- }
-
+ ros::init(argc,argv,"ros_camera");
+ this->rosnode_ = new ros::NodeHandle();
}
////////////////////////////////////////////////////////////////////////////////
// Destructor
RosCamera::~RosCamera()
{
+ delete this->rosnode_;
delete this->topicNameP;
delete this->frameNameP;
}
@@ -95,7 +90,7 @@
this->frameName = this->frameNameP->GetValue();
ROS_DEBUG("================= %s", this->topicName.c_str());
- rosnode->advertise<image_msgs::Image>(this->topicName,1);
+ this->pub_ = this->rosnode_->advertise<image_msgs::Image>(this->topicName,1);
}
////////////////////////////////////////////////////////////////////////////////
@@ -140,7 +135,6 @@
// Finalize the controller
void RosCamera::FiniChild()
{
- rosnode->unadvertise(this->topicName);
this->myParent->SetActive(false);
}
@@ -171,7 +165,7 @@
//double tmpT2;
/// @todo: don't bother if there are no subscribers
- if (this->rosnode->numSubscribers(this->topicName) > 0)
+ if (this->pub_.getNumSubscribers() > 0)
{
// copy from src to imageMsg
fillImage(this->imageMsg ,"image_raw" ,
@@ -182,7 +176,7 @@
//tmpT2 = Simulator::Instance()->GetWallTime();
// publish to ros
- rosnode->publish(this->topicName,this->imageMsg);
+ this->pub_.publish(this->imageMsg);
}
//double tmpT3 = Simulator::Instance()->GetWallTime();
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_f3d.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_f3d.cpp 2009-06-24 18:00:57 UTC (rev 17602)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_f3d.cpp 2009-06-24 19:19:18 UTC (rev 17603)
@@ -49,27 +49,22 @@
RosF3D::RosF3D(Entity *parent )
: Controller(parent)
{
- this->myParent = dynamic_cast<Model*>(this->parent);
+ this->myParent = dynamic_cast<Model*>(this->parent);
- if (!this->myParent)
- gzthrow("RosF3D controller requires a Model as its parent");
+ if (!this->myParent)
+ gzthrow("RosF3D controller requires a Model as its parent");
- rosnode = ros::g_node; // comes from where?
int argc = 0;
char** argv = NULL;
- if (rosnode == NULL)
- {
- // this only works for a single camera.
- ros::init(argc,argv);
- rosnode = new ros::Node("ros_gazebo",ros::Node::DONT_HANDLE_SIGINT);
- ROS_DEBUG("Starting node in RosF3D");
- }
+ ros::init(argc,argv,"ros_f3d");
+ this->rosnode_ = new ros::NodeHandle();
}
////////////////////////////////////////////////////////////////////////////////
// Destructor
RosF3D::~RosF3D()
{
+ delete this->rosnode_;
}
////////////////////////////////////////////////////////////////////////////////
@@ -84,7 +79,7 @@
this->frameName = node->GetString("frameName", "", 1);
ROS_DEBUG("==== topic name for RosF3D ======== %s", this->topicName.c_str());
- rosnode->advertise<robot_msgs::Vector3Stamped>(this->topicName,10);
+ this->pub_ = this->rosnode_->advertise<robot_msgs::Vector3Stamped>(this->topicName,10);
}
////////////////////////////////////////////////////////////////////////////////
@@ -122,7 +117,7 @@
// << " f: " << force
// << " t: " << torque << std::endl;
// publish to ros
- rosnode->publish(this->topicName,this->vector3Msg);
+ this->pub_.publish(this->vector3Msg);
this->lock.unlock();
}
@@ -131,5 +126,4 @@
// Finalize the controller
void RosF3D::FiniChild()
{
- rosnode->unadvertise(this->topicName);
}
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_laser.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_laser.cpp 2009-06-24 18:00:57 UTC (rev 17602)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_laser.cpp 2009-06-24 19:19:18 UTC (rev 17603)
@@ -58,25 +58,21 @@
// set parent sensor to active automatically
this->myParent->SetActive(true);
- rosnode = ros::g_node; // comes from where? common.h exports as global variable
- int argc = 0;
- char** argv = NULL;
- if (rosnode == NULL)
- {
- // start a ros node if none exist
- ros::init(argc,argv);
- rosnode = new ros::Node("ros_gazebo",ros::Node::DONT_HANDLE_SIGINT);
- ROS_DEBUG("Starting node in laser");
- }
this->hokuyo_min_intensity = 101.0;
ROS_WARN("WARNING: ros_laser plugin artifically sets minimum intensity to %f due to cutoff in hokuyo filters."
, this->hokuyo_min_intensity);
+
+ int argc = 0;
+ char** argv = NULL;
+ ros::init(argc,argv,"ros_laser");
+ this->rosnode_ = new ros::NodeHandle();
}
////////////////////////////////////////////////////////////////////////////////
// Destructor
RosLaser::~RosLaser()
{
+ delete this->rosnode_;
}
////////////////////////////////////////////////////////////////////////////////
@@ -85,7 +81,7 @@
{
this->topicName = node->GetString("topicName","default_ros_laser",0); //read from xml file
ROS_DEBUG("================= %s", this->topicName.c_str());
- rosnode->advertise<laser_scan::LaserScan>(this->topicName,10);
+ this->pub_ = this->rosnode_->advertise<laser_scan::LaserScan>(this->topicName,10);
this->frameName = node->GetString("frameName","default_ros_laser",0); //read from xml file
this->gaussianNoise = node->GetDouble("gaussianNoise",0.0,0); //read from xml file
}
@@ -107,7 +103,6 @@
// Finalize the controller
void RosLaser::FiniChild()
{
- rosnode->unadvertise(this->topicName);
}
////////////////////////////////////////////////////////////////////////////////
@@ -182,7 +177,7 @@
}
// send data out via ros message
- rosnode->publish(this->topicName,this->laserMsg);
+ this->pub_.publish(this->laserMsg);
this->lock.unlock();
}
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_p3d.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_p3d.cpp 2009-06-24 18:00:57 UTC (rev 17602)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_p3d.cpp 2009-06-24 19:19:18 UTC (rev 17603)
@@ -51,22 +51,17 @@
if (!this->myParent)
gzthrow("RosP3D controller requires a Model as its parent");
- rosnode = ros::g_node; // comes from where?
int argc = 0;
char** argv = NULL;
- if (rosnode == NULL)
- {
- // this only works for a single camera.
- ros::init(argc,argv);
- rosnode = new ros::Node("ros_gazebo",ros::Node::DONT_HANDLE_SIGINT);
- ROS_DEBUG("Starting node in RosP3D");
- }
+ ros::init(argc,argv,"ros_p3d");
+ this->rosnode_ = new ros::NodeHandle();
}
////////////////////////////////////////////////////////////////////////////////
// Destructor
RosP3D::~RosP3D()
{
+ delete this->rosnode_;
}
////////////////////////////////////////////////////////////////////////////////
@@ -89,7 +84,7 @@
ROS_DEBUG("==== topic name for RosP3D ======== %s", this->topicName.c_str());
if (this->topicName != "")
- rosnode->advertise<robot_msgs::PoseWithRatesStamped>(this->topicName,10);
+ this->pub_ = this->rosnode_->advertise<robot_msgs::PoseWithRatesStamped>(this->topicName,10);
}
////////////////////////////////////////////////////////////////////////////////
@@ -180,7 +175,7 @@
this->poseMsg.acc.ang_acc.az = this->aeul.z + this->GaussianKernel(0,this->gaussianNoise) ;
// publish to ros
- rosnode->publish(this->topicName,this->poseMsg);
+ this->pub_.publish(this->poseMsg);
}
this->lock.unlock();
@@ -193,8 +188,6 @@
// Finalize the controller
void RosP3D::FiniChild()
{
- if (this->topicName != "")
- rosnode->unadvertise(this->topicName);
}
//////////////////////////////////////////////////////////////////////////////
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_prosilica.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_prosilica.cpp 2009-06-24 18:00:57 UTC (rev 17602)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_prosilica.cpp 2009-06-24 19:19:18 UTC (rev 17603)
@@ -96,22 +96,17 @@
this->distortion_t2P = new ParamT<double>("distortion_t2" ,0, 0);
Param::End();
- rosnode = ros::g_node; // comes from where?
int argc = 0;
char** argv = NULL;
- if (rosnode == NULL)
- {
- ros::init(argc,argv);
- rosnode = new ros::Node("ros_gazebo",ros::Node::DONT_HANDLE_SIGINT);
- ROS_DEBUG("Starting node in prosilica plugin");
- }
-
+ ros::init(argc,argv,"ros_prosilica");
+ this->rosnode_ = new ros::NodeHandle();
}
////////////////////////////////////////////////////////////////////////////////
// Destructor
RosProsilica::~RosProsilica()
{
+ delete this->rosnode_;
delete this->imageTopicNameP;
delete this->imageRectTopicNameP;
delete this->camInfoTopicNameP;
@@ -166,11 +161,11 @@
this->distortion_t2 = this->distortion_t2P->GetValue();
ROS_DEBUG("prosilica image topic name %s", this->imageTopicName.c_str());
- rosnode->advertise<image_msgs::Image>(this->imageTopicName,1);
- rosnode->advertise<image_msgs::Image>(this->imageRectTopicName,1);
- rosnode->advertise<image_msgs::CamInfo>(this->camInfoTopicName,1);
- rosnode->advertiseService(this->camInfoServiceName,&RosProsilica::camInfoService, this, 0);
- rosnode->advertiseService(this->pollServiceName,&RosProsilica::triggeredGrab, this, 0);
+ this->image_pub_ = this->rosnode_->advertise<image_msgs::Image>(this->imageTopicName,1);
+ this->image_rect_pub_ = this->rosnode_->advertise<image_msgs::Image>(this->imageRectTopicName,1);
+ this->cam_info_pub_ = this->rosnode_->advertise<image_msgs::CamInfo>(this->camInfoTopicName,1);
+ this->cam_info_ser_ = this->rosnode_->advertiseService(this->camInfoServiceName,&RosProsilica::camInfoService, this);
+ this->poll_ser_ = this->rosnode_->advertiseService(this->pollServiceName,&RosProsilica::triggeredGrab, this);
}
////////////////////////////////////////////////////////////////////////////////
@@ -315,8 +310,8 @@
(void*)src );
// publish to ros, thumbnails and rect image?
/// @todo: don't bother if there are no subscribers
- if (this->rosnode->numSubscribers(this->imageTopicName) > 0)
- rosnode->publish(this->imageTopicName,this->imageMsg);
+ if (this->image_pub_.getNumSubscribers() > 0)
+ this->image_pub_.publish(this->imageMsg);
image_msgs::CvBridge img_bridge_;
img_bridge_.fromImage(this->imageMsg,this->format.c_str());
@@ -391,11 +386,6 @@
// Finalize the controller
void RosProsilica::FiniChild()
{
- rosnode->unadvertise(this->imageTopicName);
- rosnode->unadvertise(this->imageRectTopicName);
- rosnode->unadvertise(this->camInfoTopicName);
- rosnode->unadvertiseService(this->camInfoServiceName);
- rosnode->unadvertiseService(this->pollServiceName);
this->myParent->SetActive(false);
}
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_ptz.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_ptz.cpp 2009-06-24 18:00:57 UTC (rev 17602)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_ptz.cpp 2009-06-24 19:19:18 UTC (rev 17603)
@@ -67,23 +67,17 @@
this->stateTopicNameP = new ParamT<std::string>("stateTopicName","PTZ_state",0);
Param::End();
- rosnode = ros::g_node; // comes from where?
int argc = 0;
char** argv = NULL;
- if (rosnode == NULL)
- {
- // this only works for a single camera.
- ros::init(argc,argv);
- rosnode = new ros::Node("ros_gazebo",ros::Node::DONT_HANDLE_SIGINT);
- ROS_DEBUG("Starting node in camera");
- }
-
+ ros::init(argc,argv,"ros_ptz");
+ this->rosnode_ = new ros::NodeHandle();
}
////////////////////////////////////////////////////////////////////////////////
// Destructor
RosPTZ::~RosPTZ()
{
+ delete this->rosnode_;
//if (this->panJoint)
// delete this->panJoint;
//if (this->tiltJoint)
@@ -124,8 +118,8 @@
ROS_DEBUG(" publishing state topic for ptz %s", this->stateTopicName.c_str());
ROS_DEBUG(" subscribing command topic for ptz %s", this->commandTopicName.c_str());
- rosnode->advertise<axis_cam::PTZActuatorState>(this->stateTopicName,10);
- rosnode->subscribe( commandTopicName, PTZControlMessage, &RosPTZ::PTZCommandReceived,this,10);
+ this->pub_ = this->rosnode_->advertise<axis_cam::PTZActuatorState>(this->stateTopicName,10);
+ this->sub_ = this->rosnode_->subscribe(this->commandTopicName.c_str(), 10, &RosPTZ::PTZCommandReceived,this);
if (!this->panJoint)
gzthrow("couldn't get pan hinge joint");
@@ -135,11 +129,11 @@
}
-void RosPTZ::PTZCommandReceived()
+void RosPTZ::PTZCommandReceived(const axis_cam::PTZActuatorCmdConstPtr& in)
{
this->lock.lock();
- this->cmdPan = PTZControlMessage.pan.cmd*M_PI/180.0;
- this->cmdTilt = PTZControlMessage.tilt.cmd*M_PI/180.0;
+ this->cmdPan = in->pan.cmd*M_PI/180.0;
+ this->cmdTilt = in->tilt.cmd*M_PI/180.0;
this->lock.unlock();
}
@@ -211,8 +205,6 @@
// Finalize the controller
void RosPTZ::FiniChild()
{
- rosnode->unadvertise(this->stateTopicName);
- rosnode->unsubscribe(commandTopicName); // FIXME: only unsubscribe if subscribed?
}
////////////////////////////////////////////////////////////////////////////////
@@ -229,7 +221,7 @@
PTZStateMessage.tilt.pos_valid=1;
PTZStateMessage.tilt.pos = RTOD(this->tiltJoint->GetAngle());
// publish topic
- this->rosnode->publish(this->stateTopicName,PTZStateMessage);
+ this->pub_.publish(PTZStateMessage);
this->lock.unlock();
}
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_sim_iface.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_sim_iface.cpp 2009-06-24 18:00:57 UTC (rev 17602)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_sim_iface.cpp 2009-06-24 19:19:18 UTC (rev 17603)
@@ -56,17 +56,6 @@
if (!this->myParent)
gzthrow("RosSimIface controller requires an Entity as its parent");
-
- rosnode = ros::g_node; // comes from where?
- int argc = 0;
- char** argv = NULL;
- if (rosnode == NULL)
- {
- ros::init(argc,argv);
- rosnode = new ros::Node("ros_gazebo",ros::Node::DONT_HANDLE_SIGINT);
- ROS_DEBUG("Starting node in simiface");
- }
-
Param::Begin(&this->parameters);
this->topicNameP = new ParamT<std::string>("topicName","simiface_pose", 0);
this->frameNameP = new ParamT<std::string>("frameName","map", 0);
@@ -76,12 +65,18 @@
this->velP = new ParamT<Vector3>("vel" ,Vector3(0,0,0), 0);
this->angVelP = new ParamT<Vector3>("angVel" ,Vector3(0,0,0), 0);
Param::End();
+
+ int argc = 0;
+ char** argv = NULL;
+ ros::init(argc,argv,"ros_sim_iface");
+ this->rosnode_ = new ros::NodeHandle();
}
////////////////////////////////////////////////////////////////////////////////
// Destructor
RosSimIface::~RosSimIface()
{
+ delete this->rosnode_;
delete this->topicNameP;
delete this->frameNameP;
@@ -121,19 +116,19 @@
{
ROS_DEBUG("ros simiface subscribing to %s", this->topicName.c_str());
- rosnode->subscribe( this->topicName, poseMsg, &RosSimIface::UpdateObjectPose, this, 10);
+ this->sub_ = this->rosnode_->subscribe(this->topicName.c_str(), 10, &RosSimIface::UpdateObjectPose,this);
}
////////////////////////////////////////////////////////////////////////////////
// Update the controller
-void RosSimIface::UpdateObjectPose()
+void RosSimIface::UpdateObjectPose(const robot_msgs::PoseWithRatesStampedConstPtr& poseMsg)
{
this->lock.lock();
// copy data into pose
- this->poseMsg.header.frame_id = this->frameName;
- this->poseMsg.header.stamp.fromSec(floor(Simulator::Instance()->GetSimTime()));
+ //poseMsg->header.frame_id = this->frameName;
+ //poseMsg->header.stamp.fromSec(floor(Simulator::Instance()->GetSimTime()));
gazebo::Client *client = new gazebo::Client();
gazebo::SimulationIface *simIface = new gazebo::SimulationIface();
@@ -169,13 +164,13 @@
gazebo::SimulationRequestData *request = &(simIface->data->requests[simIface->data->requestCount++]);
request->type = gazebo::SimulationRequestData::SET_POSE3D;
memcpy(request->modelName, this->modelName.c_str(), this->modelName.size());
- request->modelPose.pos.x = this->poseMsg.pos.position.x;
- request->modelPose.pos.y = this->poseMsg.pos.position.y;
- request->modelPose.pos.z = this->poseMsg.pos.position.z;
- Quatern quat = Quatern(this->poseMsg.pos.orientation.w,
- this->poseMsg.pos.orientation.x,
- this->poseMsg.pos.orientation.y,
- this->poseMsg.pos.orientation.z);
+ request->modelPose.pos.x = poseMsg->pos.position.x;
+ request->modelPose.pos.y = poseMsg->pos.position.y;
+ request->modelPose.pos.z = poseMsg->pos.position.z;
+ Quatern quat = Quatern(poseMsg->pos.orientation.w,
+ poseMsg->pos.orientation.x,
+ poseMsg->pos.orientation.y,
+ poseMsg->pos.orientation.z);
Vector3 euler = quat.GetAsEuler();
request->modelPose.roll = euler.x;
request->modelPose.pitch = euler.y;
@@ -210,7 +205,6 @@
// Finalize the controller
void RosSimIface::FiniChild()
{
- rosnode->unsubscribe(this->topicName);
}
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_stereo_camera.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_stereo_camera.cpp 2009-06-24 18:00:57 UTC (rev 17602)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_stereo_camera.cpp 2009-06-24 19:19:18 UTC (rev 17603)
@@ -79,18 +79,6 @@
this->baselineP = new ParamT<double>("baseline" ,0.05, 0); // distance from left to right camera
Param::End();
- // get ros node instance if it exists
- rosnode = ros::g_node;
- int argc = 0;
- char** argv = NULL;
- if (rosnode == NULL)
- {
- // this only works for a single camera.
- ros::init(argc,argv);
- rosnode = new ros::Node("ros_gazebo",ros::Node::DONT_HANDLE_SIGINT);
- ROS_DEBUG("Starting node in stereo camera");
- }
-
// RawStereo.msg
this->leftImageMsg = &(this->rawStereoMsg.left_image);
this->rightImageMsg = &(this->rawStereoMsg.right_image);
@@ -98,12 +86,18 @@
this->rightCamInfoMsg = &(this->rawStereoMsg.right_info);
this->stereoInfoMsg = &(this->rawStereoMsg.stereo_info);
ROS_DEBUG("stereo: done with constuctor");
+
+ int argc = 0;
+ char** argv = NULL;
+ ros::init(argc,argv,"ros_stereo_camera");
+ this->rosnode_ = new ros::NodeHandle();
}
////////////////////////////////////////////////////////////////////////////////
// Destructor
RosStereoCamera::~RosStereoCamera()
{
+ delete this->rosnode_;
delete this->leftCameraNameP;
delete this->rightCameraNameP;
delete this->frameNameP;
@@ -174,7 +168,7 @@
// advertise node topics
ROS_DEBUG("stereo: advertise topicName %s\n",this->topicName.c_str());
- rosnode->advertise<image_msgs::RawStereo>(this->topicName, 1);
+ this->pub_ = this->rosnode_->advertise<image_msgs::RawStereo>(this->topicName, 1);
// iterate through children of the model parent to find left and right camera sensors
std::vector<Entity*> sibling = this->myParent->GetChildren();
@@ -241,7 +235,6 @@
// Finalize the controller
void RosStereoCamera::FiniChild()
{
- rosnode->unadvertise(this->topicName);
this->leftCamera->SetActive(false);
this->rightCamera->SetActive(false);
}
@@ -260,7 +253,7 @@
if (left_src && right_src)
{
/// @todo: don't bother if there are no subscribers
- if (this->rosnode->numSubscribers(this->topicName) > 0)
+ if (this->pub_.getNumSubscribers() > 0)
{
this->lock.lock();
// setup header
@@ -411,7 +404,7 @@
// fill uint8 has_disparity
this->rawStereoMsg.has_disparity = 0;
// publish to ros
- rosnode->publish(this->topicName,this->rawStereoMsg);
+ this->pub_.publish(this->rawStereoMsg);
this->lock.unlock();
}
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_time.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_time.cpp 2009-06-24 18:00:57 UTC (rev 17602)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_time.cpp 2009-06-24 19:19:18 UTC (rev 17603)
@@ -48,28 +48,25 @@
: Controller(parent)
{
- rosnode_ = ros::g_node; // comes from where?
- int argc = 0;
- char** argv = NULL;
- if (rosnode_ == NULL)
- {
- // this only works for a single camera.
- ros::init(argc,argv);
- rosnode_ = new ros::Node("ros_gazebo",ros::Node::DONT_HANDLE_SIGINT);
- ROS_DEBUG("Starting node in RosTime");
- }
+ int argc = 0;
+ char** argv = NULL;
+ ros::init(argc,argv,"ros_time");
- // for rostime
- rosnode_->advertise<roslib::Time>("time",10);
+ this->rosnode_ = new ros::NodeHandle();
- // broadcasting sim time, so set parameter
- rosnode_->setParam("/use_sim_time", true);
+ // for rostime
+ this->pub_ = this->rosnode_->advertise<roslib::Time>("time",10);
+
+ // broadcasting sim time, so set parameter, this should really be in the launch script param tag, so it's set before nodes start
+ this->rosnode_->setParam("/use_sim_time", true);
+
}
////////////////////////////////////////////////////////////////////////////////
// Destructor
RosTime::~RosTime()
{
+ delete this->rosnode_;
}
////////////////////////////////////////////////////////////////////////////////
@@ -101,7 +98,7 @@
this->lock.lock();
timeMsg.rostime.sec = (unsigned long)floor(currentTime);
timeMsg.rostime.nsec = (unsigned long)floor( 1e9 * ( currentTime - timeMsg.rostime.sec) );
- rosnode_->publish("time",timeMsg);
+ this->pub_.publish(timeMsg);
this->lock.unlock();
}
@@ -109,7 +106,6 @@
// Finalize the controller
void RosTime::FiniChild()
{
- rosnode_->unadvertise("time");
}
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2factory.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2factory.cpp 2009-06-24 18:00:57 UTC (rev 17602)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2factory.cpp 2009-06-24 19:19:18 UTC (rev 17603)
@@ -41,7 +41,7 @@
#include <gazebo/gazebo.h>
#include <gazebo/GazeboError.hh>
-#include "ros/node.h"
+#include "ros/ros.h"
#include <urdf/URDF.h>
@@ -123,7 +123,7 @@
/// Open the Factory interface
try
{
- factoryIface->Open(client, "factory_iface");
+ factoryIface->Open(client, "factory_model::factory_iface");
}
catch (gazebo::GazeboError e)
{
@@ -135,11 +135,11 @@
std::string xml_param_name = std::string(argv[1]);
// Load parameter server string for pr2 robot description
- ros::init(argc,argv);
- ros::Node* rosnode = new ros::Node(xml_param_name.c_str(),ros::Node::DONT_HANDLE_SIGINT);
+ ros::init(argc,argv,"urdf2factory");
+ ros::NodeHandle rosnode;
ROS_INFO("-------------------- starting node for pr2 param server factory \n");
std::string xml_content;
- rosnode->getParam(xml_param_name.c_str(),xml_content);
+ rosnode.getParam(xml_param_name.c_str(),xml_content);
ROS_DEBUG("%s content\n%s\n", xml_param_name.c_str(), xml_content.c_str());
// Parse URDF from param server
@@ -212,8 +212,6 @@
factoryIface->Unlock();
}
- rosnode->shutdown();
-
return 0;
}
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/xml2factory.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/xml2factory.cpp 2009-06-24 18:00:57 UTC (rev 17602)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/xml2factory.cpp 2009-06-24 19:19:18 UTC (rev 17603)
@@ -41,7 +41,7 @@
#include <gazebo/gazebo.h>
#include <gazebo/GazeboError.hh>
-#include "ros/node.h"
+#include "ros/ros.h"
void usage(const char *progname)
{
@@ -117,7 +117,7 @@
/// Open the Factory interface
try
{
- factoryIface->Open(client, "factory_iface");
+ factoryIface->Open(client, "factory_model::factory_iface");
}
catch (gazebo::GazeboError e)
{
@@ -129,11 +129,11 @@
std::string xml_param_name = std::string(argv[1]);
// Load parameter server string for pr2 robot description
- ros::init(argc,argv);
- ros::Node* rosnode = new ros::Node(xml_param_name.c_str(),ros::Node::DONT_HANDLE_SIGINT);
+ ros::init(argc,argv,"xml2factory");
+ ros::NodeHandle rosnode;
ROS_INFO("-------------------- starting node for pr2 param server factory \n");
std::string xml_content;
- rosnode->getParam(xml_param_name.c_str(),xml_content);
+ rosnode.getParam(xml_param_name.c_str(),xml_content);
ROS_DEBUG("%s content\n%s\n", xml_param_name.c_str(), xml_content.c_str());
if (xml_content.c_str()==NULL)
@@ -183,8 +183,6 @@
factoryIface->Unlock();
}
- rosnode->shutdown();
-
return 0;
}
Modified: pkg/trunk/stacks/simulators/gazebo/Makefile
===================================================================
--- pkg/trunk/stacks/simulators/gazebo/Makefile 2009-06-24 18:00:57 UTC (rev 17602)
+++ pkg/trunk/stacks/simulators/gazebo/Makefile 2009-06-24 19:19:18 UTC (rev 17603)
@@ -5,8 +5,8 @@
SVN_DIR = gazebo-svn
SVN_URL = https://playerstage.svn.sourceforge.net/svnroot/playerstage/code/gazebo/trunk
-SVN_REVISION = -r 7853
-SVN_PATCH =
+SVN_REVISION = -r 7888
+SVN_PATCH = gazebo_new_patch.diff
include $(shell rospack find mk)/svn_checkout.mk
#SVN_URL = https://playerstage.svn.sourceforge.net/svnroot/playerstage/code/gazebo/branches/ogre-1.4.9
Added: pkg/trunk/stacks/simulators/gazebo/gazebo_new_patch.diff
===================================================================
--- pkg/trunk/stacks/simulators/gazebo/gazebo_new_patch.diff (rev 0)
+++ pkg/trunk/stacks/simulators/gazebo/gazebo_new_patch.diff 2009-06-24 19:19:18 UTC (rev 17603)
@@ -0,0 +1,102 @@
+Index: server/physics/Body.hh
+===================================================================
+--- server/physics/Body.hh (revision 7888)
++++ server/physics/Body.hh (working copy)
+@@ -259,6 +259,8 @@
+ protected: ParamT<double> *iyzP;
+ protected: bool customMassMatrix;
+ protected: double cx,cy,cz,bodyMass,ixx,iyy,izz,ixy,ixz,iyz;
++
++ private: std::string lightName;
+ };
+
+ /// \}
+Index: server/physics/Geom.cc
+===================================================================
+--- server/physics/Geom.cc (revision 7888)
++++ server/physics/Geom.cc (working copy)
+@@ -250,7 +250,7 @@
+
+ if (this->placeable && !this->IsStatic())
+ {
+- if (dGeomGetClass(geomId) != dTriMeshClass)
++ //if (dGeomGetClass(geomId) != dTriMeshClass)
+ {
+ this->transId = dCreateGeomTransform( this->spaceId );
+ dGeomTransformSetGeom( this->transId, this->geomId );
+Index: server/physics/Body.cc
+===================================================================
+--- server/physics/Body.cc (revision 7888)
++++ server/physics/Body.cc (working copy)
+@@ -242,6 +242,10 @@
+ }
+ }
+
++ if ((childNode = node->GetChild("light")))
++ {
++ this->lightName = OgreCreator::CreateLight(childNode, this->cgVisual);
++ }
+
+ childNode = node->GetChildByNSPrefix("sensor");
+
+Index: worlds/bandit.world
+===================================================================
+--- worlds/bandit.world (revision 7888)
++++ worlds/bandit.world (working copy)
+@@ -16,7 +16,7 @@
+ <verbosity>5</verbosity>
+
+ <physics:ode>
+- <stepTime>0.03</stepTime>
++ <stepTime>0.01</stepTime>
+ <gravity>0 0 -9.8</gravity>
+ <cfm>10e-5</cfm>
+ <erp>0.3</erp>
+@@ -58,7 +58,7 @@
+ </include>
+
+ <model:physical name="box_model">
+- <xyz>0.15 0 0.125</xyz>
++ <xyz>0.05 0 0.125</xyz>
+ <static>false</static>
+
+ <attach>
+Index: worlds/test_stacks_with_rays.world
+===================================================================
+--- worlds/test_stacks_with_rays.world (revision 7888)
++++ worlds/test_stacks_with_rays.world (working copy)
+@@ -28,7 +28,7 @@
+ <cfm>0.0000000001</cfm>
+ <erp>0.2</erp>
+ <quickStep>true</quickStep>
+- <quickStepIters>10</quickStepIters>
++ <quickStepIters>100</quickStepIters>
+ <quickStepW>1.3</quickStepW>
+ </physics:ode>
+
+Index: worlds/test_stacks.world
+===================================================================
+--- worlds/test_stacks.world (revision 7888)
++++ worlds/test_stacks.world (working copy)
+@@ -28,7 +28,7 @@
+ <cfm>0.0000000001</cfm>
+ <erp>0.1</erp>
+ <quickStep>true</quickStep>
+- <quickStepIters>30</quickStepIters>
++ <quickStepIters>20</quickStepIters>
+ <quickStepW>1.3</quickStepW>
+ </physics:ode>
+
+Index: CMakeLists.txt
+===================================================================
+--- CMakeLists.txt (revision 7888)
++++ CMakeLists.txt (working copy)
+@@ -30,7 +30,7 @@
+ SET (gazebocontroller_sources_desc "List of controller sources"
+ CACHE INTERNAL "Gazebo controller sources list description" FORCE)
+
+-SET (OGRE_VERSION 1.6.2 CACHE INTERNAL "Ogre version requirement" FORCE)
++SET (OGRE_VERSION 1.6.1 CACHE INTERNAL "Ogre version requirement" FORCE)
+
+ SET (FREEIMAGE_MAJOR_VERSION 3 CACHE INTERNAL "FreeImage major version requirement" FORCE)
+ SET (FREEIMAGE_MINOR_VERSION 10 CACHE INTERNAL "FreeImage minor version requirement" FORCE)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
