Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[4294] pkg/trunk
|
From: <hsu...@us...> - 2008-09-15 20:54:36
|
Revision: 4294
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4294&view=rev
Author: hsujohnhsu
Date: 2008-09-15 20:54:42 +0000 (Mon, 15 Sep 2008)
Log Message:
-----------
moved odometry frame publishing to base controller.
moved odometry message publishing from BaseController to BaseControllerNode.
removed corresponding functions from test_actuators.
Modified Paths:
--------------
pkg/trunk/controllers/pr2_controllers/include/pr2_controllers/base_controller.h
pkg/trunk/controllers/pr2_controllers/manifest.xml
pkg/trunk/controllers/pr2_controllers/src/base_controller.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/test_actuators.h
pkg/trunk/drivers/simulator/gazebo_plugin/src/test_actuators.cpp
Modified: pkg/trunk/controllers/pr2_controllers/include/pr2_controllers/base_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_controllers/include/pr2_controllers/base_controller.h 2008-09-15 20:32:00 UTC (rev 4293)
+++ pkg/trunk/controllers/pr2_controllers/include/pr2_controllers/base_controller.h 2008-09-15 20:54:42 UTC (rev 4294)
@@ -55,6 +55,8 @@
#include <std_msgs/RobotBase2DOdom.h>
#include <std_msgs/BaseVel.h>
+#include <rosTF/rosTF.h>
+
#include <pthread.h>
namespace controller
@@ -163,16 +165,15 @@
/*!
- * \brief Set the publish count (number of update ticks between odometry message publishing).
+ * \brief returns odometry data
*/
- void setPublishCount(int publish_count);
+ void getOdometry(double &x, double &y, double &w, double &vx, double &vy, double &vw);
/*!
- * \brief returns odometry data
+ * \brief set odometry message
*/
- void getOdometry(double &x, double &y, double &w, double &vx, double &vy, double &vw);
+ void setOdomMessage(std_msgs::RobotBase2DOdom &odom_msg_);
-
private:
/*!
@@ -186,15 +187,7 @@
*/
int num_casters_;
-
/*!
- * \brief number of update ticks to wait before publishing ROS odom message
- * defaults to 10.
- */
- int odom_publish_count_;
-
-
- /*!
* \brief local gain used for speed control of the caster (to achieve resultant position control)
*/
double kp_speed_;
@@ -297,21 +290,9 @@
std::vector<double> steer_angle_actual_; /** vector of actual caster steer angles */
-
std::vector<double> wheel_speed_actual_; /** vector of actual wheel speeds */
-
- /*!
- * \brief std_msgs representation of an odometry message
- */
- std_msgs::RobotBase2DOdom odom_msg_;
-
-
double last_time_; /** time corresponding to when update was last called */
-
-
- int odom_publish_counter_; /** counter - when this exceeds odom_publish_count_, the odomeetry message will be published on ROS */
-
};
class BaseControllerNode : public Controller
@@ -350,13 +331,36 @@
* \brief deal with cmd_vel command from 2dnav stack
*/
void CmdBaseVelReceived();
- std_msgs::BaseVel velMsg;
+ std_msgs::BaseVel baseVelMsg;
/*!
* \brief mutex lock for setting and getting ros messages
*/
ros::thread::mutex ros_lock_;
+ /*!
+ * \brief frame transform server for publishing base odometry frame
+ */
+ private: rosTFServer *tfs;
+
+ /*!
+ * \brief Set the publish count (number of update ticks between odometry message publishing).
+ */
+ void setPublishCount(int publish_count); //FIXME: use time rather than count
+
+ /*!
+ * \brief std_msgs representation of an odometry message
+ */
+ std_msgs::RobotBase2DOdom odom_msg_;
+
+ /*!
+ * \brief number of update ticks to wait before publishing ROS odom message
+ * defaults to 10.
+ */
+ int odom_publish_count_; //FIXME: use time rather than count
+
+
+ int odom_publish_counter_; /** counter - when this exceeds odom_publish_count_, the odomeetry message will be published on ROS */ //FIXME: use time rather than count
};
/** \brief A namespace ostream overload for displaying parameters */
Modified: pkg/trunk/controllers/pr2_controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/pr2_controllers/manifest.xml 2008-09-15 20:32:00 UTC (rev 4293)
+++ pkg/trunk/controllers/pr2_controllers/manifest.xml 2008-09-15 20:54:42 UTC (rev 4294)
@@ -11,6 +11,7 @@
<depend package="generic_controllers" />
<depend package="rospy" />
<depend package="libTF" />
+ <depend package="rosTF" />
<depend package="std_msgs" />
<depend package="math_utils" />
<depend package="robot_kinematics" />
Modified: pkg/trunk/controllers/pr2_controllers/src/base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_controllers/src/base_controller.cpp 2008-09-15 20:32:00 UTC (rev 4293)
+++ pkg/trunk/controllers/pr2_controllers/src/base_controller.cpp 2008-09-15 20:54:42 UTC (rev 4294)
@@ -43,7 +43,7 @@
ROS_REGISTER_CONTROLLER(BaseController)
-BaseController::BaseController() : num_wheels_(0), num_casters_(0), odom_publish_count_(10), odom_publish_counter_(0)
+BaseController::BaseController() : num_wheels_(0), num_casters_(0)
{
cmd_vel_.x = 0;
cmd_vel_.y = 0;
@@ -88,19 +88,12 @@
return cmd_vel;
}
-void BaseController::setPublishCount(int publish_count)
-{
- odom_publish_count_ = publish_count;
-};
-
-
void BaseController::init(std::vector<JointControlParam> jcp, mechanism::RobotState *robot_state)
{
std::vector<JointControlParam>::iterator jcp_iter;
robot_desc::URDF::Link *link;
std::string joint_name;
-// ros::g_node->advertise<std_msgs::RobotBase2DOdom>("odom");
std::string xml_content;
(ros::g_node)->get_param("robotdesc/pr2",xml_content);
if(!urdf_model_.loadString(xml_content.c_str()))
@@ -291,17 +284,9 @@
computeOdometry(current_time);
- if(odom_publish_counter_ > odom_publish_count_) // FIXME: time based rate limiting
- {
- (ros::g_node)->publish("odom", odom_msg_);
- odom_publish_counter_ = 0;
- }
-
updateJointControllers();
last_time_ = current_time;
-
- odom_publish_counter_++;
}
void BaseController::computeAndSetCasterSteer()
@@ -362,9 +347,26 @@
base_casters_[i].controller_.update();
}
+void BaseController::setOdomMessage(std_msgs::RobotBase2DOdom &odom_msg_)
+{
+ odom_msg_.header.frame_id = "FRAMEID_ODOM";
+ odom_msg_.header.stamp.sec = (unsigned long)floor(robot_state_->hw_->current_time_);
+ odom_msg_.header.stamp.nsec = (unsigned long)floor( 1e9 * ( robot_state_->hw_->current_time_ - odom_msg_.header.stamp.sec) );
+ odom_msg_.pos.x = base_odom_position_.x;
+ odom_msg_.pos.y = base_odom_position_.y;
+ odom_msg_.pos.th = base_odom_position_.z;
+
+ odom_msg_.vel.x = base_odom_velocity_.x;
+ odom_msg_.vel.y = base_odom_velocity_.y;
+ odom_msg_.vel.th = base_odom_velocity_.z;
+
+// cout << "Base Odometry: Velocity " << base_odom_velocity_;
+// cout << "Base Odometry: Position " << base_odom_position_;
+}
+
ROS_REGISTER_CONTROLLER(BaseControllerNode)
- BaseControllerNode::BaseControllerNode()
+ BaseControllerNode::BaseControllerNode() : odom_publish_count_(10), odom_publish_counter_(0)
{
c_ = new BaseController();
}
@@ -374,9 +376,65 @@
delete c_;
}
+void BaseControllerNode::setPublishCount(int publish_count)
+{
+ odom_publish_count_ = publish_count;
+};
+
void BaseControllerNode::update()
{
c_->update();
+
+ c_->setOdomMessage(odom_msg_);
+
+ if(odom_publish_counter_ > odom_publish_count_) // FIXME: switch to time based rate limiting
+ {
+ (ros::g_node)->publish("odom", odom_msg_);
+ odom_publish_counter_ = 0;
+
+ // FIXME: should this be in here?
+ /***************************************************************/
+ /* */
+ /* frame transforms */
+ /* */
+ /* TODO: should we send z, roll, pitch, yaw? seems to confuse */
+ /* localization */
+ /* */
+ /***************************************************************/
+ double x=0,y=0,z=0,roll=0,pitch=0,yaw=0,vx,vy,vyaw;
+ this->getOdometry(x,y,yaw,vx,vy,vyaw);
+ // FIXME: z, roll, pitch not accounted for
+ this->tfs->sendInverseEuler("FRAMEID_ODOM",
+ "base",
+ x,
+ y,
+ 0, //z - base_center_offset_z, /* get infor from xml: half height of base box */
+ yaw,
+ pitch,
+ roll,
+ odom_msg_.header.stamp);
+
+ /***************************************************************/
+ /* */
+ /* frame transforms */
+ /* */
+ /* x,y,z,yaw,pitch,roll */
+ /* */
+ /***************************************************************/
+ this->tfs->sendEuler("base",
+ "FRAMEID_ROBOT",
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ odom_msg_.header.stamp);
+
+ }
+
+ odom_publish_counter_++;
+
}
bool BaseControllerNode::setCommand(
@@ -434,15 +492,18 @@
node->advertise<std_msgs::RobotBase2DOdom>("odom");
// receive messages from 2dnav stack
- node->subscribe("cmd_vel", velMsg, &BaseControllerNode::CmdBaseVelReceived, this);
+ node->subscribe("cmd_vel", baseVelMsg, &BaseControllerNode::CmdBaseVelReceived, this);
+ // for publishing odometry frame transforms FRAMEID_ODOM
+ this->tfs = new rosTFServer(*node); //, true, 1 * 1000000000ULL, 0ULL);
+
return true;
}
void BaseControllerNode::CmdBaseVelReceived()
{
this->ros_lock_.lock();
- this->setCommand(velMsg.vx,0.0,velMsg.vw);
+ this->setCommand(baseVelMsg.vx,0.0,baseVelMsg.vw);
this->ros_lock_.unlock();
}
@@ -497,21 +558,6 @@
base_odom_delta.z = base_odom_velocity_.z * dt;
base_odom_position_ += base_odom_delta;
-
- odom_msg_.header.frame_id = "FRAMEID_ODOM";
- odom_msg_.header.stamp.sec = (unsigned long)floor(robot_state_->hw_->current_time_);
- odom_msg_.header.stamp.nsec = (unsigned long)floor( 1e9 * ( robot_state_->hw_->current_time_ - odom_msg_.header.stamp.sec) );
-
- odom_msg_.pos.x = base_odom_position_.x;
- odom_msg_.pos.y = base_odom_position_.y;
- odom_msg_.pos.th = base_odom_position_.z;
-
- odom_msg_.vel.x = base_odom_velocity_.x;
- odom_msg_.vel.y = base_odom_velocity_.y;
- odom_msg_.vel.th = base_odom_velocity_.z;
-
-// cout << "Base Odometry: Velocity " << base_odom_velocity_;
-// cout << "Base Odometry: Position " << base_odom_position_;
}
void BaseController::computeBaseVelocity()
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/test_actuators.h
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/test_actuators.h 2008-09-15 20:32:00 UTC (rev 4293)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/test_actuators.h 2008-09-15 20:54:42 UTC (rev 4294)
@@ -49,8 +49,6 @@
// roscpp - used for broadcasting time over ros
#include <rostools/Time.h>
// ros messages
-//#include <std_msgs/RobotBase2DOdom.h>
-//#include <std_msgs/BaseVel.h>
//#include <std_msgs/PR2Arm.h>
//#include <pr2_msgs/EndEffectorState.h>
@@ -85,7 +83,6 @@
virtual void FiniChild();
// Callbacks for subscribed messages
- //void CmdBaseVelReceived();
//void CmdLeftarmconfigReceived();
//void CmdRightarmconfigReceived();
//void CmdLeftarmcartesianReceived();
@@ -108,7 +105,6 @@
private:
- //std_msgs::BaseVel velMsg;
// arm joint data
//std_msgs::PR2Arm leftarmMsg;
//std_msgs::PR2Arm rightarmMsg;
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/test_actuators.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/test_actuators.cpp 2008-09-15 20:32:00 UTC (rev 4293)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/test_actuators.cpp 2008-09-15 20:54:42 UTC (rev 4294)
@@ -554,7 +554,6 @@
//rosnode_->advertise<std_msgs::PR2Arm>("right_pr2arm_pos");
rosnode_->advertise<rostools::Time>("time");
- //rosnode_->subscribe("cmd_vel", velMsg, &TestActuators::CmdBaseVelReceived, this);
//rosnode_->subscribe("cmd_leftarmconfig", leftarmMsg, &TestActuators::CmdLeftarmconfigReceived,this);
//rosnode_->subscribe("cmd_rightarmconfig", rightarmMsg, &TestActuators::CmdRightarmconfigReceived,this);
//rosnode_->subscribe("cmd_leftarm_cartesian", leftarmcartesianMsg, &TestActuators::CmdLeftarmcartesianReceived);
@@ -600,51 +599,15 @@
{
- // FIXME: the frame transforms should be published by individual mechanism joints, not here
- // FIXME: the frame transforms should be published by individual mechanism joints, not here
- // FIXME: the frame transforms should be published by individual mechanism joints, not here
- /***************************************************************/
- /* */
- /* frame transforms */
- /* */
- /* TODO: should we send z, roll, pitch, yaw? seems to confuse */
- /* localization */
- /* */
- /***************************************************************/
- double x=0,y=0,z=0,roll=0,pitch=0,yaw=0,vx,vy,vyaw;
- controller::Controller* bc = mc_.getControllerByName( "base_controller" );
- controller::BaseControllerNode* bcn = dynamic_cast<controller::BaseControllerNode*>(bc);
- if (bcn) bcn->getOdometry(x,y,yaw,vx,vy,vyaw);
- // FIXME: z, roll, pitch not accounted for
- tfs->sendInverseEuler("FRAMEID_ODOM",
- "base",
- x,
- y,
- z, //z - base_center_offset_z, /* get infor from xml: half height of base box */
- yaw,
- pitch,
- roll,
- timeMsg.rostime);
-
- /***************************************************************/
- /* */
- /* frame transforms */
- /* */
- /* x,y,z,yaw,pitch,roll */
- /* */
- /***************************************************************/
- tfs->sendEuler("base",
- "FRAMEID_ROBOT",
- 0,
- 0,
- 0,
- 0,
- 0,
- 0,
- timeMsg.rostime);
-
-
+ /***********************************************************************************************/
+ /* */
+ /* frame transforms for laser sensors */
+ /* */
+ /* FIXME mechanism transform should treat <sensor> in pr2.xml same way <link> is treated, */
+ /* i.e. publish frame transform for <sensor> as well. */
+ /* */
+ /***********************************************************************************************/
// base laser location
tfs->sendEuler("base_laser",
"base",
@@ -752,22 +715,7 @@
this->lock.unlock();
}
-
void
- TestActuators::CmdBaseVelReceived()
- {
- this->lock.lock();
- controller::Controller* cc = mc_.getControllerByName( "base_controller" );
- controller::BaseControllerNode* bc = dynamic_cast<controller::BaseControllerNode*>(cc);
- if (bc)
- bc->setCommand(velMsg.vx,0.0,velMsg.vw);
- this->lock.unlock();
- }
-
-
-
-
- void
TestActuators::CmdLeftarmcartesianReceived()
{
this->lock.lock();
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
