Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[4290] pkg/trunk
|
From: <hsu...@us...> - 2008-09-15 20:03:07
|
Revision: 4290
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4290&view=rev
Author: hsujohnhsu
Date: 2008-09-15 20:03:11 +0000 (Mon, 15 Sep 2008)
Log Message:
-----------
moved cmd_vel subscription to base controller.
commentd out arm message stuff from test_actuators.
Modified Paths:
--------------
pkg/trunk/controllers/pr2_controllers/include/pr2_controllers/base_controller.h
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 19:14:42 UTC (rev 4289)
+++ pkg/trunk/controllers/pr2_controllers/include/pr2_controllers/base_controller.h 2008-09-15 20:03:11 UTC (rev 4290)
@@ -53,6 +53,7 @@
#include <newmat10/newmatap.h>
#include <std_msgs/RobotBase2DOdom.h>
+#include <std_msgs/BaseVel.h>
#include <pthread.h>
@@ -155,7 +156,6 @@
*/
pthread_mutex_t base_controller_lock_;
-
/*!
* \brief URDF representation of the robot model
*/
@@ -346,6 +346,17 @@
BaseController *c_;
+ /*!
+ * \brief deal with cmd_vel command from 2dnav stack
+ */
+ void CmdBaseVelReceived();
+ std_msgs::BaseVel velMsg;
+
+ /*!
+ * \brief mutex lock for setting and getting ros messages
+ */
+ ros::thread::mutex ros_lock_;
+
};
/** \brief A namespace ostream overload for displaying parameters */
Modified: pkg/trunk/controllers/pr2_controllers/src/base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_controllers/src/base_controller.cpp 2008-09-15 19:14:42 UTC (rev 4289)
+++ pkg/trunk/controllers/pr2_controllers/src/base_controller.cpp 2008-09-15 20:03:11 UTC (rev 4290)
@@ -433,9 +433,19 @@
node->advertise<std_msgs::RobotBase2DOdom>("odom");
+ // receive messages from 2dnav stack
+ node->subscribe("cmd_vel", velMsg, &BaseControllerNode::CmdBaseVelReceived, this);
+
return true;
}
+void BaseControllerNode::CmdBaseVelReceived()
+{
+ this->ros_lock_.lock();
+ this->setCommand(velMsg.vx,0.0,velMsg.vw);
+ this->ros_lock_.unlock();
+}
+
void BaseControllerNode::getOdometry(double &x, double &y, double &w, double &vx, double &vy, double &vw)
{
c_->getOdometry(x,y,w,vx,vy,vw);
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 19:14:42 UTC (rev 4289)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/test_actuators.h 2008-09-15 20:03:11 UTC (rev 4290)
@@ -49,10 +49,10 @@
// 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>
+//#include <std_msgs/RobotBase2DOdom.h>
+//#include <std_msgs/BaseVel.h>
+//#include <std_msgs/PR2Arm.h>
+//#include <pr2_msgs/EndEffectorState.h>
// Ioan's parser
#include <urdf/URDF.h>
@@ -85,11 +85,11 @@
virtual void FiniChild();
// Callbacks for subscribed messages
- void CmdBaseVelReceived();
- void CmdLeftarmconfigReceived();
- void CmdRightarmconfigReceived();
- void CmdLeftarmcartesianReceived();
- void CmdRightarmcartesianReceived();
+ //void CmdBaseVelReceived();
+ //void CmdLeftarmconfigReceived();
+ //void CmdRightarmconfigReceived();
+ //void CmdLeftarmcartesianReceived();
+ //void CmdRightarmcartesianReceived();
#if 0
bool reset_IK_guess(gazebo_plugin::VoidVoid::request &req, gazebo_plugin::VoidVoid::response &res);
bool SetRightArmCartesian(gazebo_plugin::MoveCartesian::request &req, gazebo_plugin::MoveCartesian::response &res);
@@ -108,13 +108,13 @@
private:
- std_msgs::BaseVel velMsg;
- // arm joint data
- std_msgs::PR2Arm leftarmMsg;
- std_msgs::PR2Arm rightarmMsg;
+ //std_msgs::BaseVel velMsg;
+ // arm joint data
+ //std_msgs::PR2Arm leftarmMsg;
+ //std_msgs::PR2Arm rightarmMsg;
// end effector cmds
- pr2_msgs::EndEffectorState leftarmcartesianMsg;
- pr2_msgs::EndEffectorState rightarmcartesianMsg;
+ //pr2_msgs::EndEffectorState leftarmcartesianMsg;
+ //pr2_msgs::EndEffectorState rightarmcartesianMsg;
rostools::Time timeMsg;
@@ -163,7 +163,7 @@
// for storing reverse transmission results
//mechanism::Robot* reverse_mech_robot_;
- std_msgs::PR2Arm larm,rarm;
+ //std_msgs::PR2Arm larm,rarm;
// for storing controller xml
struct Gazebo_joint_
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/test_actuators.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/test_actuators.cpp 2008-09-15 19:14:42 UTC (rev 4289)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/test_actuators.cpp 2008-09-15 20:03:11 UTC (rev 4290)
@@ -441,29 +441,29 @@
rosnode_->publish("time",timeMsg);
- // FIXME: move to arm controller
+ // FIXEDME: deprecated by mechanism state messages!!!
/* get left arm position */
- if( mc_.state_->getJointState("shoulder_pan_left_joint") ) larm.turretAngle = mc_.state_->getJointState("shoulder_pan_left_joint") ->position_;
- if( mc_.state_->getJointState("shoulder_pitch_left_joint") ) larm.shoulderLiftAngle = mc_.state_->getJointState("shoulder_pitch_left_joint")->position_;
- if( mc_.state_->getJointState("upperarm_roll_left_joint") ) larm.upperarmRollAngle = mc_.state_->getJointState("upperarm_roll_left_joint") ->position_;
- if( mc_.state_->getJointState("elbow_flex_left_joint") ) larm.elbowAngle = mc_.state_->getJointState("elbow_flex_left_joint") ->position_;
- if( mc_.state_->getJointState("forearm_roll_left_joint") ) larm.forearmRollAngle = mc_.state_->getJointState("forearm_roll_left_joint") ->position_;
- if( mc_.state_->getJointState("wrist_flex_left_joint") ) larm.wristPitchAngle = mc_.state_->getJointState("wrist_flex_left_joint") ->position_;
- if( mc_.state_->getJointState("gripper_roll_left_joint") ) larm.wristRollAngle = mc_.state_->getJointState("gripper_roll_left_joint") ->position_;
- if( mc_.state_->getJointState("gripper_left_joint") ) larm.gripperForceCmd = mc_.state_->getJointState("gripper_left_joint") ->applied_effort_;
- if( mc_.state_->getJointState("gripper_left_joint") ) larm.gripperGapCmd = mc_.state_->getJointState("gripper_left_joint") ->position_;
- rosnode_->publish("left_pr2arm_pos", larm);
- /* get right arm position */
- if( mc_.state_->getJointState("shoulder_pan_right_joint") ) rarm.turretAngle = mc_.state_->getJointState("shoulder_pan_right_joint") ->position_;
- if( mc_.state_->getJointState("shoulder_pitch_right_joint") ) rarm.shoulderLiftAngle = mc_.state_->getJointState("shoulder_pitch_right_joint")->position_;
- if( mc_.state_->getJointState("upperarm_roll_right_joint") ) rarm.upperarmRollAngle = mc_.state_->getJointState("upperarm_roll_right_joint") ->position_;
- if( mc_.state_->getJointState("elbow_flex_right_joint") ) rarm.elbowAngle = mc_.state_->getJointState("elbow_flex_right_joint") ->position_;
- if( mc_.state_->getJointState("forearm_roll_right_joint") ) rarm.forearmRollAngle = mc_.state_->getJointState("forearm_roll_right_joint") ->position_;
- if( mc_.state_->getJointState("wrist_flex_right_joint") ) rarm.wristPitchAngle = mc_.state_->getJointState("wrist_flex_right_joint") ->position_;
- if( mc_.state_->getJointState("gripper_roll_right_joint") ) rarm.wristRollAngle = mc_.state_->getJointState("gripper_roll_right_joint") ->position_;
- if( mc_.state_->getJointState("gripper_right_joint") ) rarm.gripperForceCmd = mc_.state_->getJointState("gripper_right_joint") ->applied_effort_;
- if( mc_.state_->getJointState("gripper_right_joint") ) rarm.gripperGapCmd = mc_.state_->getJointState("gripper_right_joint") ->position_;
- rosnode_->publish("right_pr2arm_pos", rarm);
+ // if( mc_.state_->getJointState("shoulder_pan_left_joint") ) larm.turretAngle = mc_.state_->getJointState("shoulder_pan_left_joint") ->position_;
+ // if( mc_.state_->getJointState("shoulder_pitch_left_joint") ) larm.shoulderLiftAngle = mc_.state_->getJointState("shoulder_pitch_left_joint")->position_;
+ // if( mc_.state_->getJointState("upperarm_roll_left_joint") ) larm.upperarmRollAngle = mc_.state_->getJointState("upperarm_roll_left_joint") ->position_;
+ // if( mc_.state_->getJointState("elbow_flex_left_joint") ) larm.elbowAngle = mc_.state_->getJointState("elbow_flex_left_joint") ->position_;
+ // if( mc_.state_->getJointState("forearm_roll_left_joint") ) larm.forearmRollAngle = mc_.state_->getJointState("forearm_roll_left_joint") ->position_;
+ // if( mc_.state_->getJointState("wrist_flex_left_joint") ) larm.wristPitchAngle = mc_.state_->getJointState("wrist_flex_left_joint") ->position_;
+ // if( mc_.state_->getJointState("gripper_roll_left_joint") ) larm.wristRollAngle = mc_.state_->getJointState("gripper_roll_left_joint") ->position_;
+ // if( mc_.state_->getJointState("gripper_left_joint") ) larm.gripperForceCmd = mc_.state_->getJointState("gripper_left_joint") ->applied_effort_;
+ // if( mc_.state_->getJointState("gripper_left_joint") ) larm.gripperGapCmd = mc_.state_->getJointState("gripper_left_joint") ->position_;
+ // rosnode_->publish("left_pr2arm_pos", larm);
+ // /* get right arm position */
+ // if( mc_.state_->getJointState("shoulder_pan_right_joint") ) rarm.turretAngle = mc_.state_->getJointState("shoulder_pan_right_joint") ->position_;
+ // if( mc_.state_->getJointState("shoulder_pitch_right_joint") ) rarm.shoulderLiftAngle = mc_.state_->getJointState("shoulder_pitch_right_joint")->position_;
+ // if( mc_.state_->getJointState("upperarm_roll_right_joint") ) rarm.upperarmRollAngle = mc_.state_->getJointState("upperarm_roll_right_joint") ->position_;
+ // if( mc_.state_->getJointState("elbow_flex_right_joint") ) rarm.elbowAngle = mc_.state_->getJointState("elbow_flex_right_joint") ->position_;
+ // if( mc_.state_->getJointState("forearm_roll_right_joint") ) rarm.forearmRollAngle = mc_.state_->getJointState("forearm_roll_right_joint") ->position_;
+ // if( mc_.state_->getJointState("wrist_flex_right_joint") ) rarm.wristPitchAngle = mc_.state_->getJointState("wrist_flex_right_joint") ->position_;
+ // if( mc_.state_->getJointState("gripper_roll_right_joint") ) rarm.wristRollAngle = mc_.state_->getJointState("gripper_roll_right_joint") ->position_;
+ // if( mc_.state_->getJointState("gripper_right_joint") ) rarm.gripperForceCmd = mc_.state_->getJointState("gripper_right_joint") ->applied_effort_;
+ // if( mc_.state_->getJointState("gripper_right_joint") ) rarm.gripperGapCmd = mc_.state_->getJointState("gripper_right_joint") ->position_;
+ // rosnode_->publish("right_pr2arm_pos", rarm);
PublishFrameTransforms();
@@ -550,13 +550,13 @@
int
TestActuators::AdvertiseSubscribeMessages()
{
- rosnode_->advertise<std_msgs::PR2Arm>("left_pr2arm_pos");
- rosnode_->advertise<std_msgs::PR2Arm>("right_pr2arm_pos");
+ //rosnode_->advertise<std_msgs::PR2Arm>("left_pr2arm_pos");
+ //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_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);
//rosnode_->subscribe("cmd_rightarm_cartesian", rightarmcartesianMsg, &TestActuators::CmdRightarmcartesianReceived);
@@ -676,7 +676,7 @@
-
+#if 0
void
TestActuators::CmdLeftarmconfigReceived()
{
@@ -767,7 +767,6 @@
-#if 0
void
TestActuators::CmdLeftarmcartesianReceived()
{
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
