Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[18338] pkg/trunk
|
From: <mp...@us...> - 2009-07-06 20:54:14
|
Revision: 18338
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18338&view=rev
Author: mppics
Date: 2009-07-06 20:30:57 +0000 (Mon, 06 Jul 2009)
Log Message:
-----------
Added teleop_anti_collision, updated controllers/odom and keyboard teleop gripper
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_kinematics.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_base_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/src/base_kinematics.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_base_controller.cpp
pkg/trunk/sandbox/pr2_gripper_controller/src/teleop_gripper_keyboard.cpp
pkg/trunk/stacks/pr2/pr2_default_controllers/pr2_base_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/pr2_base_odometry.xml
Added Paths:
-----------
pkg/trunk/sandbox/teleop_anti_collision/
pkg/trunk/sandbox/teleop_anti_collision/.build_version
pkg/trunk/sandbox/teleop_anti_collision/CMakeLists.txt
pkg/trunk/sandbox/teleop_anti_collision/Makefile
pkg/trunk/sandbox/teleop_anti_collision/bin/
pkg/trunk/sandbox/teleop_anti_collision/launch/
pkg/trunk/sandbox/teleop_anti_collision/launch/base_odom_teleop.xml
pkg/trunk/sandbox/teleop_anti_collision/launch/run_teleop_anti_collision.xml
pkg/trunk/sandbox/teleop_anti_collision/launch/test_teleop_anti_collision.xml
pkg/trunk/sandbox/teleop_anti_collision/lib/
pkg/trunk/sandbox/teleop_anti_collision/manifest.xml
pkg/trunk/sandbox/teleop_anti_collision/src/
pkg/trunk/sandbox/teleop_anti_collision/src/teleop_goal_projection.cpp
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_kinematics.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_kinematics.h 2009-07-06 20:23:46 UTC (rev 18337)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_kinematics.h 2009-07-06 20:30:57 UTC (rev 18338)
@@ -31,7 +31,7 @@
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
- /*
+/*
* Author: Sachin Chitta and Matthew Piccoli
*/
@@ -43,245 +43,245 @@
namespace controller
{
- class Wheel;
- class Caster;
- class BaseKinematics;
- /*! \class
- \brief This class keeps track of the wheels
- */
- class Wheel
- {
- public:
-
- /*!
- * \brief JointState for this wheel joint
- */
- mechanism::JointState *joint_;
-
- /*!
- * \brief default offset from the parent caster before any transformations
- */
- robot_msgs::Point offset_;
-
- /*!
- * \brief name of the joint
- */
- std::string name_;
- /*!
- * \brief offset_ after rotation transformation from the parent caster's position
- */
- robot_msgs::Point position_;
-
- /*!
- * \brief the caster on which this wheel is mounted
- */
- Caster *parent_;
-
- /*!
- * \brief actual wheel speeds
- */
- double wheel_speed_actual_;
-
- /*!
- * \brief desired wheel speed
- */
- double wheel_speed_cmd_;
-
- /*!
- * \brief difference between desired and actual speed
- */
- double wheel_speed_error_;
-
- /*!
- * \brief wheel speed filtered with alpha
- */
- double wheel_speed_filtered_;
-
- /*!
- * \brief specifies the default direction of the wheel
- */
- int direction_multiplier_;
-
- /*!
- * \brief remembers if the wheel is stalled
- */
- int wheel_stuck_;
-
- /*!
- * \brief wheel radius scale (based on the default wheel radius in Basekinematics)
- */
- double wheel_radius_scaler_;
-
- /*!
- * \brief Loads wheel's information from the xml description file and param server
- * @param robot_state The robot's current state
- * @param config Tiny xml element pointing to this wheel
- */
- void initXml(mechanism::RobotState *robot_state, TiXmlElement *config);
-
- /*!
- * \brief Computes 2d postion of the wheel relative to the center of the base
- */
- void updatePosition();
- };
-
- /*! \class
- \brief This class keeps track of the casters
- */
- class Caster
- {
- public:
-
- /*!
- * \brief JointState for this caster joint
- */
- mechanism::JointState *joint_;
-
- /*!
- * \brief offset from the center of the base
- */
- robot_msgs::Point offset_;
-
- /*!
- * \brief name of the joint
- */
- std::string name_;
- //robot_msgs::Point position_;
-
- /*!
- * \brief BaseKinematics to which this caster belongs
- */
- BaseKinematics *parent_;
-
- /*!
- * \brief the number of child wheels that are attached to this caster
- */
- int num_children_;
-
- /*!
- * \brief actual caster steer angles
- */
- double steer_angle_actual_;
-
- /*!
- * \brief vector of desired caster steer speeds
- */
- double steer_velocity_desired_;
-
- /*!
- * \brief stored caster steer angles
- */
- double steer_angle_stored_;
-
- /*!
- * \brief difference between desired and actual angles of the casters
- */
- double caster_position_error_;
-
- /*!
- * \brief difference between desired and actual speeds of the casters
- */
- double caster_speed_error_;
-
- /*!
- * \brief caster speed filtered with alpha
- */
- double caster_speed_filtered_;
+ class Wheel;
+ class Caster;
+ class BaseKinematics;
+ /*! \class
+ \brief This class keeps track of the wheels
+ */
+ class Wheel
+ {
+ public:
- /*!
- * \brief remembers the caster's current speed
- */
- double caster_speed_;
-
- /*!
- * \brief remembers if the caster is stalled
- */
- int caster_stuck_;
-
- /*!
- * \brief Loads caster's information from the xml description file and param server
- * @param robot_state The robot's current state
- * @param config Tiny xml element pointing to this caster
- */
- void initXml(mechanism::RobotState *robot_state, TiXmlElement *config);
- };
-
- /*! \class
- \brief This class includes common functions used by the base controller and odometry
- */
- class BaseKinematics
- {
- public:
-
- /*!
- * \brief Loads BaseKinematic's information from the xml description file and param server
- * @param robot_state The robot's current state
- * @param config Tiny xml element pointing to its controller
- * @return Successful init
- */
- bool initXml(mechanism::RobotState *robot_state, TiXmlElement *config);
-
- /*!
- * \brief Computes 2d postion of every wheel relative to the center of the base
- */
- void computeWheelPositions();
-
- /*!
- * \brief Computes 2d velocity of a point at relative distance pos to another point with velocity (and rotation (z)) vel
- * @param pos The position of the object relative to the center of rotation
- * @param vel Velocity of the center of rotation
- * @return Velocity at the given point
- */
- robot_msgs::PoseDot pointVel2D(const robot_msgs::Point& pos, const robot_msgs::PoseDot& vel);
-
- /*!
- * \brief remembers everything about the state of the robot
- */
- mechanism::RobotState *robot_state_;
-
- /*!
- * \brief number of wheels connected to the base
- */
- int num_wheels_;
-
- /*!
- * \brief number of casters connected to the base
- */
- int num_casters_;
-
- /*!
- * \brief vector of every wheel attached to the base
- */
- std::vector<Wheel> wheel_;
-
- /*!
- * \brief vector of every caster attached to the base
- */
- std::vector<Caster> caster_;
-
- /*!
- * \brief default radius of each wheel
- */
- double wheel_radius_;
-
- /*!
- * \brief the name of the casters in the xml file
- */
- std::string xml_caster_name_;
-
- /*!
- * \brief the name of the wheels in the xml file
- */
- std::string xml_wheel_name_;
-
- /*!
- * \brief name of this BaseKinematics (generally associated with whatever controller is using it)
- */
- std::string name_;
-
- /*!
- * \brief maximum dT used in computation of interpolated velocity command
- */
- double MAX_DT_;
- };
+ /*!
+ * \brief JointState for this wheel joint
+ */
+ mechanism::JointState *joint_;
+
+ /*!
+ * \brief default offset from the parent caster before any transformations
+ */
+ robot_msgs::Point offset_;
+
+ /*!
+ * \brief name of the joint
+ */
+ std::string name_;
+ /*!
+ * \brief offset_ after rotation transformation from the parent caster's position
+ */
+ robot_msgs::Point position_;
+
+ /*!
+ * \brief the caster on which this wheel is mounted
+ */
+ Caster *parent_;
+
+ /*!
+ * \brief actual wheel speeds
+ */
+ double wheel_speed_actual_;
+
+ /*!
+ * \brief desired wheel speed
+ */
+ double wheel_speed_cmd_;
+
+ /*!
+ * \brief difference between desired and actual speed
+ */
+ double wheel_speed_error_;
+
+ /*!
+ * \brief wheel speed filtered with alpha
+ */
+ double wheel_speed_filtered_;
+
+ /*!
+ * \brief specifies the default direction of the wheel
+ */
+ int direction_multiplier_;
+
+ /*!
+ * \brief remembers if the wheel is stalled
+ */
+ int wheel_stuck_;
+
+ /*!
+ * \brief wheel radius scale (based on the default wheel radius in Basekinematics)
+ */
+ double wheel_radius_scaler_;
+
+ /*!
+ * \brief Loads wheel's information from the xml description file and param server
+ * @param robot_state The robot's current state
+ * @param config Tiny xml element pointing to this wheel
+ */
+ void initXml(mechanism::RobotState *robot_state, TiXmlElement *config);
+
+ /*!
+ * \brief Computes 2d postion of the wheel relative to the center of the base
+ */
+ void updatePosition();
+ };
+
+ /*! \class
+ \brief This class keeps track of the casters
+ */
+ class Caster
+ {
+ public:
+
+ /*!
+ * \brief JointState for this caster joint
+ */
+ mechanism::JointState *joint_;
+
+ /*!
+ * \brief offset from the center of the base
+ */
+ robot_msgs::Point offset_;
+
+ /*!
+ * \brief name of the joint
+ */
+ std::string name_;
+ //robot_msgs::Point position_;
+
+ /*!
+ * \brief BaseKinematics to which this caster belongs
+ */
+ BaseKinematics *parent_;
+
+ /*!
+ * \brief the number of child wheels that are attached to this caster
+ */
+ int num_children_;
+
+ /*!
+ * \brief actual caster steer angles
+ */
+ double steer_angle_actual_;
+
+ /*!
+ * \brief vector of desired caster steer speeds
+ */
+ double steer_velocity_desired_;
+
+ /*!
+ * \brief stored caster steer angles
+ */
+ double steer_angle_stored_;
+
+ /*!
+ * \brief difference between desired and actual angles of the casters
+ */
+ double caster_position_error_;
+
+ /*!
+ * \brief difference between desired and actual speeds of the casters
+ */
+ double caster_speed_error_;
+
+ /*!
+ * \brief caster speed filtered with alpha
+ */
+ double caster_speed_filtered_;
+
+ /*!
+ * \brief remembers the caster's current speed
+ */
+ double caster_speed_;
+
+ /*!
+ * \brief remembers if the caster is stalled
+ */
+ int caster_stuck_;
+
+ /*!
+ * \brief Loads caster's information from the xml description file and param server
+ * @param robot_state The robot's current state
+ * @param config Tiny xml element pointing to this caster
+ */
+ void initXml(mechanism::RobotState *robot_state, TiXmlElement *config);
+ };
+
+ /*! \class
+ \brief This class includes common functions used by the base controller and odometry
+ */
+ class BaseKinematics
+ {
+ public:
+
+ /*!
+ * \brief Loads BaseKinematic's information from the xml description file and param server
+ * @param robot_state The robot's current state
+ * @param config Tiny xml element pointing to its controller
+ * @return Successful init
+ */
+ bool initXml(mechanism::RobotState *robot_state, TiXmlElement *config);
+
+ /*!
+ * \brief Computes 2d postion of every wheel relative to the center of the base
+ */
+ void computeWheelPositions();
+
+ /*!
+ * \brief Computes 2d velocity of a point at relative distance pos to another point with velocity (and rotation (z)) vel
+ * @param pos The position of the object relative to the center of rotation
+ * @param vel Velocity of the center of rotation
+ * @return Velocity at the given point
+ */
+ robot_msgs::PoseDot pointVel2D(const robot_msgs::Point& pos, const robot_msgs::PoseDot& vel);
+
+ /*!
+ * \brief remembers everything about the state of the robot
+ */
+ mechanism::RobotState *robot_state_;
+
+ /*!
+ * \brief number of wheels connected to the base
+ */
+ int num_wheels_;
+
+ /*!
+ * \brief number of casters connected to the base
+ */
+ int num_casters_;
+
+ /*!
+ * \brief vector of every wheel attached to the base
+ */
+ std::vector<Wheel> wheel_;
+
+ /*!
+ * \brief vector of every caster attached to the base
+ */
+ std::vector<Caster> caster_;
+
+ /*!
+ * \brief default radius of each wheel
+ */
+ double wheel_radius_;
+
+ /*!
+ * \brief the name of the casters in the xml file
+ */
+ std::string xml_caster_name_;
+
+ /*!
+ * \brief the name of the wheels in the xml file
+ */
+ std::string xml_wheel_name_;
+
+ /*!
+ * \brief name of this BaseKinematics (generally associated with whatever controller is using it)
+ */
+ std::string name_;
+
+ /*!
+ * \brief maximum dT used in computation of interpolated velocity command
+ */
+ double MAX_DT_;
+ };
}
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_base_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_base_controller.h 2009-07-06 20:23:46 UTC (rev 18337)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_base_controller.h 2009-07-06 20:30:57 UTC (rev 18338)
@@ -38,6 +38,7 @@
#include <ros/node.h>
#include <realtime_tools/realtime_publisher.h>
#include <pr2_msgs/BaseControllerState.h>
+#include <pr2_msgs/BaseCmdVelRad.h>
#include <robot_mechanism_controllers/joint_velocity_controller.h>
#include <pr2_mechanism_controllers/base_kinematics.h>
#include <robot_msgs/PoseDDot.h>
@@ -263,11 +264,26 @@
void CmdBaseVelReceived();
/*!
+ * \brief deal with cmd_vel command from 2dnav stack
+ */
+ void CmdBaseVelRadReceived();
+
+ /*!
+ * \brief converts from a turn radius and velocity (along tangent) to velocities in vx, vy and ang_vz
+ */
+ robot_msgs::PoseDot convertBaseVel(pr2_msgs::BaseCmdVelRad cmd_vel_rad);
+
+ /*!
* \brief callback message, used to remember where the base is commanded to go
*/
robot_msgs::PoseDot baseVelMsg;
/*!
+ * \brief callback message, used to remember where the base is commanded to go
+ */
+ pr2_msgs::BaseCmdVelRad baseVelRadMsg;
+
+ /*!
* \brief generic epsilon value that is used as a minimum or maximum allowable input value
*/
double eps_;
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_kinematics.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_kinematics.cpp 2009-07-06 20:23:46 UTC (rev 18337)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_kinematics.cpp 2009-07-06 20:30:57 UTC (rev 18338)
@@ -31,14 +31,14 @@
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
- /*
+/*
* Author: Sachin Chitta and Matthew Piccoli
*/
-
- #include <pr2_mechanism_controllers/base_kinematics.h>
-
- using namespace controller;
-
+
+#include <pr2_mechanism_controllers/base_kinematics.h>
+
+using namespace controller;
+
void Wheel::initXml(mechanism::RobotState *robot_state, TiXmlElement *config)
{
wheel_stuck_ = 0;
@@ -55,11 +55,9 @@
offset_.x = offset.getOrigin().x();
offset_.y = offset.getOrigin().y();
offset_.z = offset.getOrigin().z();
- ros::Node::instance()->param<double>(name_ + "/wheel_radius_scaler",wheel_radius_scaler_,1.0);
- #if 0
- ROS_DEBUG("Loading wheel: %s",name_.c_str());
- ROS_DEBUG("offset_.x: %f, offset_.y: %f, offset_.z: %f", offset_.x, offset_.y, offset_.z);
- #endif
+ ros::Node::instance()->param<double> (name_ + "/wheel_radius_scaler", wheel_radius_scaler_, 1.0);
+ ROS_DEBUG("Loading wheel: %s",name_.c_str());
+ ROS_DEBUG("offset_.x: %f, offset_.y: %f, offset_.z: %f", offset_.x, offset_.y, offset_.z);
}
void Caster::initXml(mechanism::RobotState *robot_state, TiXmlElement *config)
@@ -73,8 +71,7 @@
steer_velocity_desired_ = 0;
steer_angle_actual_ = 0;
num_children_ = 0;
-
- name_ = config->Attribute("name");
+
mechanism::Link *link = robot_state->model_->getLink(config->Attribute("name"));
name_ = link->joint_name_;
joint_ = robot_state->getJointState(name_);
@@ -86,10 +83,11 @@
ROS_DEBUG("Loading caster: %s: my parent is: %s",name_.c_str(), parent_->name_.c_str());
ROS_DEBUG("offset_.x: %f, offset_.y: %f, offset_.z: %f", offset_.x, offset_.y, offset_.z);
- while(elt){
+ while(elt)
+ {
Wheel tmp;
parent_->wheel_.push_back(tmp);
- parent_->wheel_[parent_->num_wheels_].initXml(robot_state,elt);
+ parent_->wheel_[parent_->num_wheels_].initXml(robot_state, elt);
elt = elt->NextSiblingElement(parent_->xml_wheel_name_);
parent_->num_wheels_++;
num_children_++;
@@ -103,29 +101,30 @@
MAX_DT_ = 0.01;
num_wheels_ = 0;
num_casters_ = 0;
-
+
//Add parameters
- ros::Node::instance()->param<std::string>("~xml_wheel_name",xml_wheel_name_,"wheel");
- ros::Node::instance()->param<std::string>("~xml_caster_name",xml_caster_name_,"caster");
- ros::Node::instance()->param<double>("~wheel_radius",wheel_radius_,0.074792);
+ ros::Node::instance()->param<std::string> ("~xml_wheel_name", xml_wheel_name_, "wheel");
+ ros::Node::instance()->param<std::string> ("~xml_caster_name", xml_caster_name_, "caster");
+ ros::Node::instance()->param<double> ("~wheel_radius", wheel_radius_, 0.074792);
double multiplier;
- ros::Node::instance()->param<double>(name_ + "/wheel_radius_multiplier",multiplier,1.0);
- wheel_radius_ = wheel_radius_*multiplier;
-
+ ros::Node::instance()->param<double> (name_ + "/wheel_radius_multiplier", multiplier, 1.0);
+ wheel_radius_ = wheel_radius_ * multiplier;
+
TiXmlElement *elt = config->FirstChildElement(xml_caster_name_);
- while(elt){
+ while(elt)
+ {
Caster tmp;
caster_.push_back(tmp);
caster_[num_casters_].parent_ = this;
- caster_[num_casters_].initXml(robot_state,elt);
+ caster_[num_casters_].initXml(robot_state, elt);
elt = elt->NextSiblingElement(xml_caster_name_);
num_casters_++;
}
int wheel_counter = 0;
- for(int j=0; j < num_casters_; j++)
+ for(int j = 0; j < num_casters_; j++)
{
- for(int i = 0; i< caster_[j].num_children_; i++)
+ for(int i = 0; i < caster_[j].num_children_; i++)
{
wheel_[wheel_counter].parent_ = &(caster_[j]);
wheel_counter++;
@@ -140,18 +139,18 @@
robot_msgs::Point result = parent_->offset_;
double costh = cos(parent_->joint_->position_);
double sinth = sin(parent_->joint_->position_);
- result.x += costh*offset_.x-sinth*offset_.y;
- result.y += sinth*offset_.x+costh*offset_.y;
+ result.x += costh * offset_.x - sinth * offset_.y;
+ result.y += sinth * offset_.x + costh * offset_.y;
result.z = 0.0;
position_ = result;
}
void BaseKinematics::computeWheelPositions()
{
- for(int i=0; i < num_wheels_; i++)
- {
- wheel_[i].updatePosition();
- }
+ for(int i = 0; i < num_wheels_; i++)
+ {
+ wheel_[i].updatePosition();
+ }
}
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_base_controller.cpp 2009-07-06 20:23:46 UTC (rev 18337)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_base_controller.cpp 2009-07-06 20:30:57 UTC (rev 18338)
@@ -101,27 +101,17 @@
ros::Node::instance()->param<double> (base_kin_.name_ + "/kp_wheel_steer_", kp_wheel_steer_, 2.0);
ros::Node::instance()->param<double> (base_kin_.name_ + "/alpha_stall", alpha_stall_, 0.5);
ros::Node::instance()->param<double> (base_kin_.name_ + "/kp_caster_steer_", kp_caster_steer_, 40.0);
- ros::Node::instance()->param<double> (base_kin_.name_ + "/timeout", timeout_, 1.0);
+ ros::Node::instance()->param<double> (base_kin_.name_ + "/timeout", timeout_, 0.1);
ros::Node::instance()->param<double> (base_kin_.name_ + "/eps", eps_, 1e-5);
ros::Node::instance()->param<double> (base_kin_.name_ + "/cmd_vel_trans_eps", cmd_vel_trans_eps_, 1e-5);
ros::Node::instance()->param<double> (base_kin_.name_ + "/cmd_vel_rot_eps", cmd_vel_rot_eps_, 1e-5);
-
- /*double kp_;
- double ki_;
- double kd_;
- double ki_clamp;*/
+ ROS_INFO("timeout is %f, base_kin_.name is %s", timeout_, base_kin_.name_.c_str());
//casters
caster_controller_.resize(base_kin_.num_casters_);
for(int i = 0; i < base_kin_.num_casters_; i++)
{
control_toolbox::Pid p_i_d;
state_publisher_->msg_.joint_name[i] = base_kin_.caster_[i].name_;
- /*ros::Node::instance()->param<double>(base_kin_.caster_[i].name_ + "/p",kp_,3.0);
- ros::Node::instance()->param<double>(base_kin_.caster_[i].name_ + "/i",ki_,0.1);
- ros::Node::instance()->param<double>(base_kin_.caster_[i].name_ + "/d",kd_,0.0);
- ros::Node::instance()->param<double>(base_kin_.caster_[i].name_ + "/i_clamp",ki_clamp,4.0);*/
- //tmp.init(base_kin_.robot_state_, base_kin_.caster_[i].name_ + "joint", control_toolbox::Pid(kp_,ki_,kd_,ki_clamp));
- //caster_controller_.push_back(tmp);//TODO::make this not copy!!!!!!!
p_i_d.initParam(base_kin_.name_ + "/" + base_kin_.caster_[i].name_ + "/");
caster_controller_[i] = new JointVelocityController();
caster_controller_[i]->init(base_kin_.robot_state_, base_kin_.caster_[i].name_, p_i_d);
@@ -132,12 +122,6 @@
{
control_toolbox::Pid p_i_d;
state_publisher_->msg_.joint_name[j + base_kin_.num_casters_] = base_kin_.wheel_[j].name_;
- /*ros::Node::instance()->param<double>(base_kin_.wheel_[j].name_ + "/kp",kp_,2.0);
- ros::Node::instance()->param<double>(base_kin_.wheel_[j].name_ + "/ki",ki_,0.01);
- ros::Node::instance()->param<double>(base_kin_.wheel_[j].name_ + "/kd",kd_,0.0);
- ros::Node::instance()->param<double>(base_kin_.wheel_[j].name_ + "/i_clamp",ki_clamp,0.4);*/
- //tmp.init(base_kin_.robot_state_, base_kin_.wheel_[j].name_ + "joint", control_toolbox::Pid(kp_,ki_,kd_,ki_clamp));
- //wheel_controller_.push_back(tmp);//TODO::make this not copy!!!!!!!
p_i_d.initParam(base_kin_.name_ + "/" + base_kin_.wheel_[j].name_ + "/");
wheel_controller_[j] = new JointVelocityController();
wheel_controller_[j]->init(base_kin_.robot_state_, base_kin_.wheel_[j].name_, p_i_d);
@@ -242,6 +226,7 @@
{
if(!base_kin_.caster_[i].joint_->calibrated_)
{
+ ROS_WARN("casters are not calibrated");
return; // Casters are not calibrated
}
}
Modified: pkg/trunk/sandbox/pr2_gripper_controller/src/teleop_gripper_keyboard.cpp
===================================================================
--- pkg/trunk/sandbox/pr2_gripper_controller/src/teleop_gripper_keyboard.cpp 2009-07-06 20:23:46 UTC (rev 18337)
+++ pkg/trunk/sandbox/pr2_gripper_controller/src/teleop_gripper_keyboard.cpp 2009-07-06 20:30:57 UTC (rev 18338)
@@ -40,7 +40,7 @@
#include <termios.h>
#include <pr2_mechanism_controllers/GripperControllerCmd.h>
-//TODO::would these #defines conflict with others? Change to const int?
+//TODO::#ifndef PACKAGE_PATH_FILE_H me!
#define KEYCODE_0 0x30
#define KEYCODE_1 0x31
#define KEYCODE_2 0x32
Added: pkg/trunk/sandbox/teleop_anti_collision/.build_version
===================================================================
--- pkg/trunk/sandbox/teleop_anti_collision/.build_version (rev 0)
+++ pkg/trunk/sandbox/teleop_anti_collision/.build_version 2009-07-06 20:30:57 UTC (rev 18338)
@@ -0,0 +1 @@
+:
Added: pkg/trunk/sandbox/teleop_anti_collision/CMakeLists.txt
===================================================================
--- pkg/trunk/sandbox/teleop_anti_collision/CMakeLists.txt (rev 0)
+++ pkg/trunk/sandbox/teleop_anti_collision/CMakeLists.txt 2009-07-06 20:30:57 UTC (rev 18338)
@@ -0,0 +1,11 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+set(ROS_BUILD_TYPE Release)
+rospack(teleop_anti_collision)
+genmsg()
+
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+
+#rospack_add_boost_directories()
+
+rospack_add_executable(teleop_goal_projection src/teleop_goal_projection.cpp)
Added: pkg/trunk/sandbox/teleop_anti_collision/Makefile
===================================================================
--- pkg/trunk/sandbox/teleop_anti_collision/Makefile (rev 0)
+++ pkg/trunk/sandbox/teleop_anti_collision/Makefile 2009-07-06 20:30:57 UTC (rev 18338)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Added: pkg/trunk/sandbox/teleop_anti_collision/launch/base_odom_teleop.xml
===================================================================
--- pkg/trunk/sandbox/teleop_anti_collision/launch/base_odom_teleop.xml (rev 0)
+++ pkg/trunk/sandbox/teleop_anti_collision/launch/base_odom_teleop.xml 2009-07-06 20:30:57 UTC (rev 18338)
@@ -0,0 +1,95 @@
+<launch>
+ <param name="base_controller/autostart" value="true"/>
+ <!--node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/base_controller.xml" output="screen"/-->
+ <param name="base_controller/odom_publish_rate" value="10" />
+ <param name="pr2_base_controller/timeout" value="0.2" />
+ <param name="pr2_base_controller/fl_caster_rotation_joint/p" value="3.0" />
+ <param name="pr2_base_controller/fr_caster_rotation_joint/p" value="3.0" />
+ <param name="pr2_base_controller/br_caster_rotation_joint/p" value="3.0" />
+ <param name="pr2_base_controller/bl_caster_rotation_joint/p" value="3.0" />
+ <param name="pr2_base_controller/fl_caster_rotation_joint/i" value="0.1" />
+ <param name="pr2_base_controller/fr_caster_rotation_joint/i" value="0.1" />
+ <param name="pr2_base_controller/br_caster_rotation_joint/i" value="0.1" />
+ <param name="pr2_base_controller/bl_caster_rotation_joint/i" value="0.1" />
+ <param name="pr2_base_controller/fl_caster_rotation_joint/d" value="0.0" />
+ <param name="pr2_base_controller/fr_caster_rotation_joint/d" value="0.0" />
+ <param name="pr2_base_controller/br_caster_rotation_joint/d" value="0.0" />
+ <param name="pr2_base_controller/bl_caster_rotation_joint/d" value="0.0" />
+ <param name="pr2_base_controller/fl_caster_rotation_joint/i_clamp" value="4.0" />
+ <param name="pr2_base_controller/fr_caster_rotation_joint/i_clamp" value="4.0" />
+ <param name="pr2_base_controller/br_caster_rotation_joint/i_clamp" value="4.0" />
+ <param name="pr2_base_controller/bl_caster_rotation_joint/i_clamp" value="4.0" />
+ <param name="pr2_base_controller/fl_caster_l_wheel_joint/p" value="2.0" />
+ <param name="pr2_base_controller/fl_caster_r_wheel_joint/p" value="2.0" />
+ <param name="pr2_base_controller/fr_caster_l_wheel_joint/p" value="2.0" />
+ <param name="pr2_base_controller/fr_caster_r_wheel_joint/p" value="2.0" />
+ <param name="pr2_base_controller/bl_caster_l_wheel_joint/p" value="2.0" />
+ <param name="pr2_base_controller/bl_caster_r_wheel_joint/p" value="2.0" />
+ <param name="pr2_base_controller/br_caster_l_wheel_joint/p" value="2.0" />
+ <param name="pr2_base_controller/br_caster_r_wheel_joint/p" value="2.0" />
+ <param name="pr2_base_controller/fl_caster_l_wheel_joint/i" value="0.01" />
+ <param name="pr2_base_controller/fl_caster_r_wheel_joint/i" value="0.01" />
+ <param name="pr2_base_controller/fr_caster_l_wheel_joint/i" value="0.01" />
+ <param name="pr2_base_controller/fr_caster_r_wheel_joint/i" value="0.01" />
+ <param name="pr2_base_controller/bl_caster_l_wheel_joint/i" value="0.01" />
+ <param name="pr2_base_controller/bl_caster_r_wheel_joint/i" value="0.01" />
+ <param name="pr2_base_controller/br_caster_l_wheel_joint/i" value="0.01" />
+ <param name="pr2_base_controller/br_caster_r_wheel_joint/i" value="0.01" />
+ <param name="pr2_base_controller/fl_caster_l_wheel_joint/d" value="0.0" />
+ <param name="pr2_base_controller/fl_caster_r_wheel_joint/d" value="0.0" />
+ <param name="pr2_base_controller/fr_caster_l_wheel_joint/d" value="0.0" />
+ <param name="pr2_base_controller/fr_caster_r_wheel_joint/d" value="0.0" />
+ <param name="pr2_base_controller/bl_caster_l_wheel_joint/d" value="0.0" />
+ <param name="pr2_base_controller/bl_caster_r_wheel_joint/d" value="0.0" />
+ <param name="pr2_base_controller/br_caster_l_wheel_joint/d" value="0.0" />
+ <param name="pr2_base_controller/br_caster_r_wheel_joint/d" value="0.0" />
+ <param name="pr2_base_controller/fl_caster_l_wheel_joint/i_clamp" value="0.4" />
+ <param name="pr2_base_controller/fl_caster_r_wheel_joint/i_clamp" value="0.4" />
+ <param name="pr2_base_controller/fr_caster_l_wheel_joint/i_clamp" value="0.4" />
+ <param name="pr2_base_controller/fr_caster_r_wheel_joint/i_clamp" value="0.4" />
+ <param name="pr2_base_controller/bl_caster_l_wheel_joint/i_clamp" value="0.4" />
+ <param name="pr2_base_controller/bl_caster_r_wheel_joint/i_clamp" value="0.4" />
+ <param name="pr2_base_controller/br_caster_l_wheel_joint/i_clamp" value="0.4" />
+ <param name="pr2_base_controller/br_caster_r_wheel_joint/i_clamp" value="0.4" />
+ <param name="pr2_base_controller/max_accel_.acc.ax" value="2.0" />
+ <param name="pr2_base_controller/max_accel_.acc.ay" value="2.0" />
+ <param name="pr2_base_controller/max_accel_.ang_acc.az" value="2.0" />
+ <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/pr2_base_controller.xml" output="screen"/>
+ <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/pr2_base_odometry.xml" output="screen"/>
+
+ <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/torso_lift_vel_controller.xml" output="screen" />
+
+ <node pkg="backup_safetysound" type="backingup.py" machine="four" />
+
+ <!-- The robot pose EKF is launched with the base controller-->
+ <param name="robot_pose_ekf/freq" value="30.0"/>
+ <param name="robot_pose_ekf/sensor_timeout" value="1.0"/>
+ <param name="robot_pose_ekf/odom_used" value="true"/>
+ <param name="robot_pose_ekf/imu_used" value="true"/>
+ <param name="robot_pose_ekf/vo_used" value="false"/>
+ <node pkg="robot_pose_ekf" type="robot_pose_ekf" args="robot_pose_ekf"></node>
+
+ <param name="axis_vx" value="3" type="int"/>
+ <param name="axis_vy" value="2" type="int"/>
+ <param name="axis_vw" value="0" type="int"/>
+ <param name="pan" value="4" type="int"/>
+ <param name="tilt" value="5" type="int"/>
+ <param name="max_vw" value="0.785" />
+ <param name="max_vx" value="1.0" />
+ <param name="max_vy" value="0.4" />
+
+ <param name="max_vw_run" value="1.4" />
+ <param name="max_vx_run" value="1.0" />
+ <param name="max_vy_run" value="1.0" />
+ <param name="run_button" value="5" type="int" />
+ <param name="torso_dn_button" value="1" type="int" />
+ <param name="torso_up_button" value="3" type="int" />
+
+ <param name="send_cmd_hz" value = "10.0" type="double" />
+ <param name="joy_msg_timeout" value="-1.0"/>
+ <param name="deadman_button" value="4" type="int"/>
+ <node pkg="teleop_anti_collision" type="teleop_goal_projection" output="screen" args="--deadman_no_publish" respawn="true" machine="four">
+ <remap from="goal" to="/move_base/activate" />
+ </node>
+
+</launch>
Added: pkg/trunk/sandbox/teleop_anti_collision/launch/run_teleop_anti_collision.xml
===================================================================
--- pkg/trunk/sandbox/teleop_anti_collision/launch/run_teleop_anti_collision.xml (rev 0)
+++ pkg/trunk/sandbox/teleop_anti_collision/launch/run_teleop_anti_collision.xml 2009-07-06 20:30:57 UTC (rev 18338)
@@ -0,0 +1,16 @@
+<?xml version="1.0" encoding="UTF-8"?>
+
+ <!-- 2dnav_anti_collision -->
+<launch>
+ <group name="wg">
+ <include file="$(find pr2_alpha)/$(env ROBOT).machine" />
+ <include file="$(find 2dnav_pr2)/config/new_amcl_node.xml" />
+ <include file="$(find teleop_anti_collision)/launch/base_odom_teleop.xml" />
+ <include file="$(find 2dnav_pr2)/config/lasers_and_filters.xml" />
+ <include file="$(find 2dnav_pr2)/config/map_server.xml" />
+ <include file="$(find 2dnav_pr2)/config/ground_plane.xml" />
+
+ <!-- The naviagtion stack and associated parameters -->
+ <include file="$(find 2dnav_pr2)/move_base/move_base.xml" />
+ </group>
+</launch>
\ No newline at end of file
Added: pkg/trunk/sandbox/teleop_anti_collision/launch/test_teleop_anti_collision.xml
===================================================================
--- pkg/trunk/sandbox/teleop_anti_collision/launch/test_teleop_anti_collision.xml (rev 0)
+++ pkg/trunk/sandbox/teleop_anti_collision/launch/test_teleop_anti_collision.xml 2009-07-06 20:30:57 UTC (rev 18338)
@@ -0,0 +1,26 @@
+<?xml version="1.0" encoding="UTF-8"?>
+
+<launch>
+
+ <!-- start up robot -->
+ <include file="$(find pr2_gazebo)/pr2_simple_office.launch"/>
+
+ <!-- Joystick -->
+ <param name="joy/deadzone" value="5000"/>
+ <node machine="four" pkg="joy" type="joy" respawn="true"/>
+
+ <!-- load map -->
+ <node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/Media/materials/textures/map_blank.png 0.1" respawn="true" machine="three" />
+ <!--node pkg="slam_gmapping" type="slam_gmapping" respawn="false" /-->
+
+ <!-- nav-stack -->
+ <include file="$(find 2dnav_gazebo)/2dnav-stack-fake_localization.launch"/>
+
+ <!-- for visualization -->
+ <!--include file="$(find 2dnav_pr2)/rviz/rviz_move_base.launch"/-->
+
+ <!-- for manual control -->
+ <!--node pkg="teleop_base" type="teleop_base_keyboard" respawn="false" output="screen" /-->
+ <!--node pkg="teleop_arm_keyboard" type="teleop_arm_keyboard" respawn="false" output="screen" /-->
+
+</launch>
Added: pkg/trunk/sandbox/teleop_anti_collision/manifest.xml
===================================================================
--- pkg/trunk/sandbox/teleop_anti_collision/manifest.xml (rev 0)
+++ pkg/trunk/sandbox/teleop_anti_collision/manifest.xml 2009-07-06 20:30:57 UTC (rev 18338)
@@ -0,0 +1,16 @@
+<package>
+ <description brief="Anti collision during teleoperation toolbox">The control toolbox contains modules that are useful for collision avoidance during teleoperation.</description>
+ <author>Matthew Piccoli</author>
+ <license>BSD</license>
+ <review status="unreviewed" notes="None"/>
+ <url>http://pr.willowgarage.com</url>
+ <repository>http://pr.willowgarage.com/repos</repository>
+ <depend package="roscpp" />
+ <depend package="roslib" />
+ <depend package="mechanism_model" />
+ <depend package="std_msgs" /><depend package="joy"></depend>
+ <export>
+ <cpp cflags="-I${prefix}/include -I${prefix}/msg/cpp"/>
+ </export>
+
+</package>
Added: pkg/trunk/sandbox/teleop_anti_collision/src/teleop_goal_projection.cpp
===================================================================
--- pkg/trunk/sandbox/teleop_anti_collision/src/teleop_goal_projection.cpp (rev 0)
+++ pkg/trunk/sandbox/teleop_anti_collision/src/teleop_goal_projection.cpp 2009-07-06 20:30:57 UTC (rev 18338)
@@ -0,0 +1,353 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * 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 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.
+ *********************************************************************/
+/*
+ * teleop_goal_projection.cpp
+ *
+ * Created on: Jun 29, 2009
+ * Author: Matthew Piccoli
+ */
+
+#include <ros/node.h>
+#include <robot_msgs/PoseStamped.h>
+#include <std_msgs/Float64.h>
+#include "joy/Joy.h"
+#include <ros/time.h>
+#include "tf/transform_listener.h"
+#include "robot_msgs/JointCmd.h"
+#define TORSO_TOPIC "/torso_lift_controller/set_command"
+#define HEAD_TOPIC "/head_controller/set_command_array"
+
+class TeleopGoalProjection: public ros::Node
+{
+ public:
+ robot_msgs::PoseStamped cmd_;
+ std_msgs::Float64 torso_vel;
+ joy::Joy joy;
+ double req_vx, req_vy, req_vw, req_torso, req_pan, req_tilt;
+ double max_vx, max_vy, max_vw;
+ double max_pan, max_tilt, min_tilt, pan_step, tilt_step;
+ int axis_vx, axis_vy, axis_vw, axis_pan, axis_tilt;
+ int deadman_button, run_button, torso_dn_button, torso_up_button, head_button, passthrough_button;
+ bool deadman_no_publish_, torso_publish, head_publish, deadman_pressed_previous_iteration;
+ ros::Time last_recieved_joy_message_time_, last_sent_command_time_;
+ ros::Duration joy_msg_timeout_;
+ ros::Duration send_cmd_time_interval_;
+ tf::TransformListener tf;
+ std::string global_frame_;
+ std::string base_frame_;
+ // Set pan, tilt steps as params
+
+ TeleopGoalProjection(bool deadman_no_publish = false) :
+ Node("teleop_goal_projection"), pan_step(0.1), tilt_step(0.1), deadman_no_publish_(deadman_no_publish)
+ {
+ torso_vel.data = 0;
+ cmd_.pose.position.x = cmd_.pose.position.y = cmd_.pose.orientation.z = 0;
+ req_pan = req_tilt = 0;
+ last_sent_command_time_ = ros::Time::now();
+ ros::Node::instance()->param("max_vx", max_vx, 0.6);
+ ros::Node::instance()->param("max_vw", max_vw, 0.8);
+ ros::Node::instance()->param("max_vy", max_vy, 0.6);
+
+ ros::Node::instance()->param("max_pan", max_pan, 2.7);
+ ros::Node::instance()->param("max_tilt", max_tilt, 1.4);
+ ros::Node::instance()->param("min_tilt", min_tilt, -0.4);
+
+ ros::Node::instance()->param("tilt_step", tilt_step, 0.1);
+ ros::Node::instance()->param("pan_step", pan_step, 0.1);
+
+ ros::Node::instance()->param("~global_frame", global_frame_, std::string("/map"));
+ ros::Node::instance()->param("~base_frame", base_frame_, std::string("/base_link"));
+
+ ros::Node::instance()->param<int> ("axis_pan", axis_pan, 0);
+ ros::Node::instance()->param<int> ("axis_tilt", axis_tilt, 2);
+
+ ros::Node::instance()->param<int> ("axis_vx", axis_vx, 3);
+ ros::Node::instance()->param<int> ("axis_vw", axis_vw, 0);
+ ros::Node::instance()->param<int> ("axis_vy", axis_vy, 2);
+
+ ros::Node::instance()->param<int> ("deadman_button", deadman_button, 0);
+ ros::Node::instance()->param<int> ("torso_dn_button", torso_dn_button, 0);
+ ros::Node::instance()->param<int> ("torso_up_button", torso_up_button, 0);
+ ros::Node::instance()->param<int> ("head_button", head_button, 0);
+
+ double send_cmd_hz;
+ ros::Node::instance()->param ("send_cmd_hz", send_cmd_hz, -1.0); //As fast as possible
+ if(send_cmd_hz <= 0)
+ {
+ send_cmd_time_interval_ = ros::Duration().fromSec(0.0);//DURATION_MAX;
+ ROS_DEBUG("send_cmd_time_interval_ <= 0 -> send commands as fast as possible");
+ }
+ else
+ {
+ send_cmd_time_interval_.fromSec(1.0/send_cmd_hz);
+ ROS_DEBUG("send_cmd_time_interval_: %.3f", send_cmd_time_interval_.toSec());
+ }
+
+ double joy_msg_timeout;
+ ros::Node::instance()->param<double> ("joy_msg_timeout", joy_msg_timeout, -1.0); //default to no timeout
+ if(joy_msg_timeout <= 0)
+ {
+ joy_msg_timeout_ = ros::Duration().fromSec(9999999);//DURATION_MAX;
+ ROS_DEBUG("joy_msg_timeout <= 0 -> no timeout");
+ }
+ else
+ {
+ joy_msg_timeout_.fromSec(joy_msg_timeout);
+ ROS_DEBUG("joy_msg_timeout: %.3f", joy_msg_timeout_.toSec());
+ }
+
+ ROS_DEBUG("max_vx: %.3f m/s\n", max_vx);
+ ROS_DEBUG("max_vy: %.3f m/s\n", max_vy);
+ ROS_DEBUG("max_vw: %.3f deg/s\n", max_vw*180.0/M_PI);
+
+ ROS_DEBUG("tilt step: %.3f rad\n", tilt_step);
+ ROS_DEBUG("pan step: %.3f rad\n", pan_step);
+
+ ROS_DEBUG("axis_vx: %d\n", axis_vx);
+ ROS_DEBUG("axis_vy: %d\n", axis_vy);
+ ROS_DEBUG("axis_vw: %d\n", axis_vw);
+ ROS_DEBUG("axis_pan: %d\n", axis_pan);
+ ROS_DEBUG("axis_tilt: %d\n", axis_tilt);
+
+ ROS_DEBUG("deadman_button: %d\n", deadman_button);
+ ROS_DEBUG("run_button: %d\n", run_button);
+ ROS_DEBUG("torso_dn_button: %d\n", torso_dn_button);
+ ROS_DEBUG("torso_up_button: %d\n", torso_up_button);
+ ROS_DEBUG("head_button: %d\n", head_button);
+ ROS_DEBUG("passthrough_button: %d\n", passthrough_button);
+ ROS_DEBUG("joy_msg_timeout: %f\n", joy_msg_timeout);
+
+ if(torso_dn_button != 0)
+ ros::Node::instance()->advertise<std_msgs::Float64> (TORSO_TOPIC, 1);
+ if(head_button != 0)
+ ros::Node::instance()->advertise<robot_msgs::JointCmd> (HEAD_TOPIC, 1);
+ ros::Node::instance()->advertise<robot_msgs::PoseStamped> ("goal", 1);
+ ros::Node::instance()->subscribe("joy", joy, &TeleopGoalProjection::joy_cb, 1);
+ ROS_DEBUG("done with ctor\n");
+ }
+
+ ~TeleopGoalProjection()
+ {
+ ros::Node::instance()->unsubscribe("joy");
+ ros::Node::instance()->unadvertise("goal");
+
+ if(torso_dn_button != 0)
+ ros::Node::instance()->unadvertise(TORSO_TOPIC);
+ if(head_button != 0)
+ ros::Node::instance()->unadvertise(HEAD_TOPIC);
+
+ }
+
+ void joy_cb()
+ {
+ //Record this message reciept
+ last_recieved_joy_message_time_ = ros::Time::now();
+
+ /*
+ printf("axes: ");
+ for(int i=0;i<joy.get_axes_size();i++)
+ printf("%.3f ", joy.axes[i]);
+ puts("");
+ printf("buttons: ");
+ for(int i=0;i<joy.get_buttons_size();i++)
+ printf("%d ", joy.buttons[i]);
+ puts("");
+ */
+ bool cmd_head = (((unsigned int) head_button < joy.get_buttons_size()) && joy.buttons[head_button]);
+
+ bool deadman = (((unsigned int) deadman_button < joy.get_buttons_size()) && joy.buttons[deadman_button]);
+
+ // Base
+ if((axis_vx >= 0) && (((unsigned int) axis_vx) < joy.get_axes_size()) && !cmd_head)
+ req_vx = joy.axes[axis_vx] * max_vx;
+ else
+ req_vx = 0.0;
+ if((axis_vy >= 0) && (((unsigned int) axis_vy) < joy.get_axes_size()) && !cmd_head)
+ req_vy = joy.axes[axis_vy] * max_vy;
+ else
+ req_vy = 0.0;
+ if((axis_vw >= 0) && (((unsigned int) axis_vw) < joy.get_axes_size()) && !cmd_head)
+ req_vw = joy.axes[axis_vw] * max_vw;
+ else
+ req_vw = 0.0;
+
+ // Head
+ // Update commanded position by how joysticks moving
+ // Don't add commanded position if deadman off
+ if((axis_pan >= 0) && (((unsigned int) axis_pan) < joy.get_axes_size()) && cmd_head && deadman)
+ {
+ req_pan += joy.axes[axis_pan] * pan_step;
+ req_pan = std::max(std::min(req_pan, max_pan), -max_pan);
+ }
+
+ if((axis_tilt >= 0) && (((unsigned int) axis_tilt) < joy.get_axes_size()) && cmd_head && deadman)
+ {
+ req_tilt += joy.axes[axis_tilt] * tilt_step;
+ req_tilt = std::max(std::min(req_tilt, max_tilt), min_tilt);
+ }
+
+ // Torso
+ bool down = (((unsigned int) torso_dn_button < joy.get_buttons_size()) && joy.buttons[torso_dn_button]);
+ bool up = (((unsigned int) torso_up_button < joy.get_buttons_size()) && joy.buttons[torso_up_button]);
+
+ // Bring torso up/down with max effort
+ if(down && !up)
+ req_torso = -0.01;
+ else if(up && !down)
+ req_torso = 0.01;
+ else
+ req_torso = 0;
+
+ }
+ void passthrough_cb()
+ {
+ }
+ void send_cmd_vel()
+ {
+ if(last_sent_command_time_ + send_cmd_time_interval_ < ros::Time::now())
+ {
+ //ROS_INFO("last %f, interval %f, now %f", last_sent_command_time_.toSec(), send_cmd_time_interval_.toSec(), ros::Time::now().toSec());
+ //ROS_INFO("last %f, interval %f, now %f", last_recieved_joy_message_time_.toSec(), joy_msg_timeout_.toSec(), ros::Time::now().toSec());
+ joy.lock();
+ if(((deadman_button < 0) || ((((unsigned int) deadman_button) < joy.get_buttons_size()) && joy.buttons[deadman_button])) && last_recieved_joy_message_time_ + joy_msg_timeout_ > ros::Time::now())
+ {
+ deadman_pressed_previous_iteration = true;
+ //ROS_INFO("!!!");
+ // use commands from the local sticks
+ //TODO::convert relative frame (base?) to global frame
+ robot_msgs::PoseStamped local_cmd;
+ local_cmd.pose.position.x = req_vx;
+ local_cmd.pose.position.y = req_vy;
+ local_cmd.pose.orientation.z = req_vw;
+ local_cmd.pose.orientation.w = sqrt(1 - local_cmd.pose.orientation.z * local_cmd.pose.orientation.z);
+ local_cmd.header.frame_id = base_frame_;
+ local_cmd.header.stamp = ros::Time();
+ tf.transformPose(global_frame_, local_cmd, cmd_);
+
+ ros::Node::instance()->publish("goal", cmd_);
+
+ // Torso
+ torso_vel.data = req_torso;
+ if(torso_dn_button != 0)
+ ros::Node::instance()->publish(TORSO_TOPIC, torso_vel);
+
+ // Head
+ if(head_button != 0)
+ {
+ robot_msgs::JointCmd joint_cmds;
+ joint_cmds.positions.push_back(req_pan);
+ joint_cmds.positions.push_back(req_tilt);
+ joint_cmds.velocity.push_back(0.0);
+ joint_cmds.velocity.push_back(0.0);
+ joint_cmds.acc.push_back(0.0);
+ joint_cmds.acc.push_back(0.0);
+ joint_cmds.names.push_back("head_pan_joint");
+ joint_cmds.names.push_back("head_tilt_joint");
+ ros::Node::instance()->publish(HEAD_TOPIC, joint_cmds);
+ }
+
+ /*if(req_torso != 0)
+ fprintf(stderr, "teleop_goal_projection:: %f, %f, %f. Head:: %f, %f. Torso effort: %f.\n", cmd_.pose.position.x, cmd_.pose.position.y, cmd_.pose.orientation.z, req_pan, req_tilt, torso_vel.data);
+ else
+ fprintf(stderr, "teleop_goal_projection:: %f, %f, %f. Head:: %f, %f\n", cmd_.pose.position.x, cmd_.pose.position.y, cmd_.pose.orientation.z, req_pan, req_tilt);*/
+ }
+ else if(deadman_pressed_previous_iteration)
+ {
+ robot_msgs::PoseStamped local_cmd;
+ local_cmd.pose.position.x = 0.0;
+ local_cmd.pose.position.y = 0.0;
+ local_cmd.pose.orientation.z = 0.0;
+ local_cmd.pose.orientation.w = sqrt(1 - local_cmd.pose.orientation.z * local_cmd.pose.orientation.z);
+ local_cmd.header.frame_id = base_frame_;
+ local_cmd.header.stamp = ros::Time();
+ tf.transformPose(global_frame_, local_cmd, cmd_);
+
+ ros::Node::instance()->publish("goal", cmd_);
+
+ deadman_pressed_previous_iteration = false;
+ }
+ else
+ {
+ cmd_.pose.position.x = cmd_.pose.position.y = cmd_.pose.orientation.z = 0;
+ torso_vel.data = 0;
+ if(!deadman_no_publish_)
+ {
+ ros::Node::instance()->publish("goal", cmd_);//Only publish if deadman_no_publish is enabled
+ if(torso_dn_button != 0)
+ ros::Node::instance()->publish(TORSO_TOPIC, torso_vel);
+
+ // Publish head
+ if(head_button != 0)
+ {
+ robot_msgs::JointCmd joint_cmds;
+ joint_cmds.positions.push_back(req_pan);
+ joint_cmds.positions.push_back(req_tilt);
+ joint_cmds.velocity.push_back(0.0);
+ joint_cmds.velocity.push_back(0.0);
+ joint_cmds.acc.push_back(0.0);
+ joint_cmds.acc.push_back(0.0);
+ joint_cmds.names.push_back("head_pan_joint");
+ joint_cmds.names.push_back("head_tilt_joint");
+ ros::Node::instance()->publish(HEAD_TOPIC, joint_cmds);
+ }
+
+ }
+ }
+ last_sent_command_time_ = ros::Time::now();
+ joy.unlock();
+ }
+ }
+};
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv);
+ const char* opt_no_publish = "--deadman_no_publish";
+
+ bool no_publish = false;
+ for(int i = 1; i < argc; i++)
+ {
+ if(!strncmp(argv[i], opt_no_publish, strlen(opt_no_publish)))
+ no_publish = true;
+ }
+ TeleopGoalProjection teleop_goal_projection(no_publish);
+ while(teleop_goal_projection.ok())
+ {
+ usleep(50000);
+ teleop_goal_projection.send_cmd_vel();
+ }
+
+ exit(0);
+ return 0;
+}
Modified: pkg/trunk/stacks/pr2/pr2_default_controllers/pr2_base_controller.xml
===================================================================
--- pkg/trunk/stacks/pr2/pr2_default_controllers/pr2_base_controller.xml 2009-07-06 20:23:46 UTC (rev 18337)
+++ pkg/trunk/stacks/pr2/pr2_default_controllers/pr2_base_controller.xml 2009-07-06 20:30:57 UTC (rev 18338)
@@ -2,21 +2,21 @@
<controllers>
<controller name="pr2_base_controller" type="Pr2BaseController">
- <caster name="fl_caster_rotation_">
- <wheel name="fl_caster_l_wheel_"/>
- <wheel name="fl_caster_r_wheel_"/>
+ <caster name="fl_caster_rotation_link">
+ <wheel name="fl_caster_l_wheel_link"/>
+ <wheel name="fl_caster_r_wheel_link"/>
</caster>
- <caster name="fr_caster_rotation_">
- <wheel name="fr_caster_l_wheel_"/>
- <wheel name="fr_caster_r_wheel_"/>
+ <caster name="fr_caster_rotation_link">
+ <wheel name="fr_caster_l_wheel_link"/>
+ <wheel name="fr_caster_r_wheel_link"/>
</caster>
- <caster name="bl_caster_rotation_">
- <wheel name="bl_caster_l_wheel_"/>
- <wheel name="bl_caster_r_wheel_"/>
+ <caster name="bl_caster_rotation_link">
+ <wheel name="bl_caster_l_wheel_link"/>
+ <wheel name="bl_caster_r_wheel_link"/>
</caster>
- <caster name="br_caster_rotation_">
- <wheel name="br_caster_l_wheel_"/>
- <wheel name="br_caster_r_wheel_"/>
+ <caster name="br_caster_rotation_link">
+ <wheel name="br_caster_l_wheel_link"/>
+ <wheel name="br_caster_r_wheel_link"/>
</caster>
</controller>
</controllers>
Modified: pkg/trunk/stacks/pr2/pr2_default_controllers/pr2_base_odometry.xml
===================================================================
--- pkg/trunk/stacks/pr2/pr2_default_controllers/pr2_base_odometry.xml 2009-07-06 20:23:46 UTC (rev 18337)
+++ pkg/trunk/stacks/pr2/pr2_default_controllers/pr2_base_odometry.xml 2009-07-06 20:30:57 UTC (rev 18338)
@@ -2,21 +2,21 @@
<controllers>
<controller name="pr2_odometry" type="Pr2Odometry">
- <caster name="fl_caster_rotation_">
- <wheel name="fl_caster_l_wheel_"/>
- <wheel name="fl_caster_r_wheel_"/>
+ <caster name="fl_caster_rotation_link">
+ <wheel name="fl_caster_l_wheel_link"/>
+ <wheel name="fl_caster_r_wheel_link"/>
</caster>
- <caster name="fr_caster_rotation_">
- <wheel name="fr_caster_l_wheel_"/>
- <wheel name="fr_caster_r_wheel_"/>
+ <caster name="fr_caster_rotation_link">
+ <wheel name="fr_caster_l_wheel_link"/>
+ <wheel name="fr_caster_r_wheel_link"/>
</caster>
- <caster name="bl_caster_rotation_">
- <wheel name="bl_caster_l_wheel_"/>
- <wheel name="bl_caster_r_wheel_"/>
+ <caster name="bl_caster_rotation_link">
+ <wheel name="bl_caster_l_wheel_link"/>
+ <wheel name="bl_caster_r_wheel_link"/>
</caster>
- <caster name="br_caster_rotation_">
- <wheel name="br_caster_l_wheel_"/>
- <wheel name="br_caster_r_wheel_"/>
+ <caster name="br_caster_rotation_link">
+ <wheel name="br_caster_l_wheel_link"/>
+ <wheel name="br_caster_r_wheel_link"/>
</caster>
</controller>
</controllers>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
