Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[14704] pkg/trunk/controllers/ pr2_mechanism_controllers
|
From: <mm...@us...> - 2009-04-30 23:59:04
|
Revision: 14704
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14704&view=rev
Author: mmwise
Date: 2009-04-30 23:58:54 +0000 (Thu, 30 Apr 2009)
Log Message:
-----------
moving PeriodicCmd srv and msg to pr2_msg/srv
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml
pkg/trunk/controllers/pr2_mechanism_controllers/scripts/send_periodic_cmd.py
pkg/trunk/controllers/pr2_mechanism_controllers/scripts/send_periodic_cmd_srv.py
pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp
Removed Paths:
-------------
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_velocity_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/msg/PeriodicCmd.msg
pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_velocity_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetPeriodicCmd.srv
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt 2009-04-30 23:56:37 UTC (rev 14703)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt 2009-04-30 23:58:54 UTC (rev 14704)
@@ -15,7 +15,6 @@
src/base_position_controller.cpp
src/laser_scanner_controller.cpp
src/laser_scanner_traj_controller.cpp
- src/laser_scanner_velocity_controller.cpp
src/arm_position_controller.cpp
src/arm_velocity_controller.cpp
src/arm_dynamics_controller.cpp
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h 2009-04-30 23:56:37 UTC (rev 14703)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h 2009-04-30 23:58:54 UTC (rev 14704)
@@ -46,14 +46,14 @@
// Messages
#include <pr2_mechanism_controllers/LaserScannerSignal.h>
-#include <pr2_mechanism_controllers/PeriodicCmd.h>
+#include <pr2_msgs/PeriodicCmd.h>
#include <pr2_mechanism_controllers/TrackLinkCmd.h>
// Services
#include <robot_mechanism_controllers/SetCommand.h>
#include <robot_mechanism_controllers/GetCommand.h>
#include <pr2_mechanism_controllers/SetProfile.h>
-#include <pr2_mechanism_controllers/SetPeriodicCmd.h>
+#include <pr2_srvs/SetPeriodicCmd.h>
#include "boost/thread/mutex.hpp"
#include "trajectory/trajectory.h"
@@ -71,7 +71,7 @@
virtual void update() ;
- bool setPeriodicCmd(const pr2_mechanism_controllers::PeriodicCmd& cmd) ;
+ bool setPeriodicCmd(const pr2_msgs::PeriodicCmd& cmd) ;
bool setTrackLinkCmd(const pr2_mechanism_controllers::TrackLinkCmd& track_link_cmd) ;
@@ -132,8 +132,8 @@
// Message Callbacks
void setPeriodicCmd() ;
void setTrackLinkCmd() ;
- bool setPeriodicSrv(pr2_mechanism_controllers::SetPeriodicCmd::Request &req,
- pr2_mechanism_controllers::SetPeriodicCmd::Response &res);
+ bool setPeriodicSrv(pr2_srvs::SetPeriodicCmd::Request &req,
+ pr2_srvs::SetPeriodicCmd::Response &res);
private:
ros::Node *node_ ;
@@ -143,7 +143,7 @@
double prev_profile_time_ ; //!< The time in the current profile when update() was last called
- pr2_mechanism_controllers::PeriodicCmd cmd_ ;
+ pr2_msgs::PeriodicCmd cmd_ ;
pr2_mechanism_controllers::TrackLinkCmd track_link_cmd_ ;
pr2_mechanism_controllers::LaserScannerSignal m_scanner_signal_ ; //!< Stores the message that we want to send at the end of each sweep, and halfway through each sweep
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_velocity_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_velocity_controller.h 2009-04-30 23:56:37 UTC (rev 14703)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_velocity_controller.h 2009-04-30 23:58:54 UTC (rev 14704)
@@ -1,211 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2008, 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.
- *********************************************************************/
-
-#pragma once
-
-#include <ros/node.h>
-
-#include <mechanism_model/controller.h>
-#include <robot_mechanism_controllers/joint_position_controller.h>
-#include <robot_mechanism_controllers/joint_velocity_controller.h>
-
-// Services
-#include <robot_mechanism_controllers/SetCommand.h>
-#include <robot_mechanism_controllers/GetCommand.h>
-#include <pr2_mechanism_controllers/SetProfile.h>
-#include <robot_mechanism_controllers/SetPosition.h>
-#include <robot_mechanism_controllers/GetPosition.h>
-namespace controller
-{
-
-class LaserScannerVelocityController : public Controller
-{
-
-public:
- //Indicates whether we close the loop around position or velocity
- enum LaserControllerMode
- {
- VELOCITY,POSITION
- };
-
- /*!
- * \brief Default Constructor of the JointController class.
- *
- */
- LaserScannerVelocityController();
-
- /*!
- * \brief Destructor of the JointController class.
- */
- ~LaserScannerVelocityController();
-
- /*!
- * \brief Functional way to initialize limits and gains.
- *
- */
- void init(double p_gain, double i_gain, double d_gain, double windup, double time, std::string name, mechanism::RobotState *robot);
- bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
-
- /*!
- * \brief Give set position of the joint for next update: revolute (angle) and prismatic (position)
- *
- * \param double pos Position command to issue
- */
- void setCommand(double command);
-
- /*!
- * \brief Get latest position command to the joint: revolute (angle) and prismatic (position).
- */
- double getCommand();
-
- /*!
- * \brief Read the torque of the motor
- */
- double getMeasuredPosition();
-
- /*!
- * \brief Issues commands to the joint. Should be called at regular intervals
- */
-
- virtual void update();
-
- /*!
- * \brief Returns the time
- */
- double getTime();
-
- double upper_turnaround_offset_; /*!<Distance from positive endstop where turnaround occurs>*/
- double lower_turnaround_offset_;/*!<Distance from negative endstop where turnaround occurs>*/
-
- double upper_deceleration_buffer_;/*!<Distance from positive turnaround where deceleration occurs>*/
- double lower_deceleration_buffer_;/*!<Distance from negative turnaround where deceleration occurs>*/
-
- bool passed_center_; /*!<Marker that indicates that we've recently moved past the center point>*/
- double last_position_; /*!<Record last read position>*/
- bool automatic_turnaround_; /*!<Do we automatically turn around at edges of workspace?>*/
-
- void setTurnaroundPoints(void);
-
- LaserControllerMode current_mode_; /*!<Indicates the current status of the controller>*/
-
- private:
-
- double upper_deceleration_zone_; /*!<Location near upper endstop where deceleration starts>*/
- double upper_turnaround_location_;/*!<Location near upper endstop where turnaround actually occurs>*/
-
- double lower_deceleration_zone_;/*!<Location near lower endstop where deceleration starts>*/
- double lower_turnaround_location_;/*!<Location near lower endstop where turnaround actually occurs>*/
-
- /*!
- * \brief Actually issue torque set command of the joint motor.
- */
- void setJointEffort(double torque);
-
- mechanism::JointState* joint_; /*!< Joint we're controlling>*/
- JointPositionController joint_position_controller_; /*!< Internal PID controller for position>*/
- JointVelocityController joint_velocity_controller_;/*!< Internal PID controller for velocity>*/
- double last_time_; /*!< Last time stamp of update> */
- double command_; /*!< Last commanded position> */
- mechanism::RobotState *robot_; /*!< Pointer to robot structure>*/
-
- };
-
-class LaserScannerVelocityControllerNode : public Controller
-{
-public:
- /*!
- * \brief Default Constructor
- *
- */
- LaserScannerVelocityControllerNode();
-
- /*!
- * \brief Destructor
- */
- ~LaserScannerVelocityControllerNode();
-
- double getMeasuredPosition();
-
- void update();
-
- bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
-
- // Services
- /*!
- * \brief Send velocity command
- */
-
- bool setCommand(robot_mechanism_controllers::SetCommand::Request &req,
- robot_mechanism_controllers::SetCommand::Response &resp);
- /*!
- * \brief Send velocity command
- */
-
- bool getCommand(robot_mechanism_controllers::GetCommand::Request &req,
- robot_mechanism_controllers::GetCommand::Response &resp);
- /*!
- * \brief Send velocity command
- */
-
- bool setPosition(robot_mechanism_controllers::SetPosition::Request &req,
- robot_mechanism_controllers::SetPosition::Response &resp);
- /*!
- * \brief Send velocity command
- */
-
- bool getPosition(robot_mechanism_controllers::GetPosition::Request &req,
- robot_mechanism_controllers::GetPosition::Response &resp);
-
- /*!
- * \brief Send velocity command
- */
-
- bool setProfile(pr2_mechanism_controllers::SetProfile::Request &req,
- pr2_mechanism_controllers::SetProfile::Response &resp);
- /*!
- * \brief Send velocity command
- */
-
- bool setProfileCall(double upper_turn_around, double lower_turn_around, double upper_decel_buffer, double lower_decel_buffer);
-
-
- void setCommand(double command);
- double getCommand();
-
-private:
- LaserScannerVelocityController *c_;
-};
-}
-
-
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml 2009-04-30 23:56:37 UTC (rev 14703)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml 2009-04-30 23:58:54 UTC (rev 14704)
@@ -16,6 +16,7 @@
<depend package="std_msgs" />
<depend package="deprecated_msgs" />
<depend package="pr2_msgs" />
+ <depend package="pr2_srvs"/>
<depend package="robot_msgs" />
<depend package="visualization_msgs" />
<depend package="robot_kinematics" />
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/msg/PeriodicCmd.msg
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/msg/PeriodicCmd.msg 2009-04-30 23:56:37 UTC (rev 14703)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/msg/PeriodicCmd.msg 2009-04-30 23:58:54 UTC (rev 14704)
@@ -1,5 +0,0 @@
-Header header
-string profile
-float64 period
-float64 amplitude
-float64 offset
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/scripts/send_periodic_cmd.py
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/scripts/send_periodic_cmd.py 2009-04-30 23:56:37 UTC (rev 14703)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/scripts/send_periodic_cmd.py 2009-04-30 23:58:54 UTC (rev 14704)
@@ -11,7 +11,7 @@
import rospy
from std_msgs import *
-from pr2_mechanism_controllers.msg import PeriodicCmd
+from pr2_msgs.msg import PeriodicCmd
from time import sleep
def print_usage(exit_code = 0):
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/scripts/send_periodic_cmd_srv.py
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/scripts/send_periodic_cmd_srv.py 2009-04-30 23:56:37 UTC (rev 14703)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/scripts/send_periodic_cmd_srv.py 2009-04-30 23:58:54 UTC (rev 14704)
@@ -11,8 +11,8 @@
import rospy
from std_msgs import *
-from pr2_mechanism_controllers.msg import PeriodicCmd
-from pr2_mechanism_controllers.srv import *
+from pr2_msgs.msg import PeriodicCmd
+from pr2_srvs.srv import *
from time import sleep
def print_usage(exit_code = 0):
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp 2009-04-30 23:56:37 UTC (rev 14703)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp 2009-04-30 23:58:54 UTC (rev 14704)
@@ -265,6 +265,7 @@
error = fabs(joint_pd_controllers_[i]->joint_state_->position_ - joint_cmd[i]);
}
return_val = return_val && (error <= goal_reached_threshold_[i]);
+ //ROS_INFO("joint error: %f threshold: %f",error, goal_reached_threshold_[i]);
}
return return_val;
}
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp 2009-04-30 23:56:37 UTC (rev 14703)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp 2009-04-30 23:58:54 UTC (rev 14704)
@@ -132,7 +132,7 @@
// Set to hold the current position
- pr2_mechanism_controllers::PeriodicCmd cmd ;
+ pr2_msgs::PeriodicCmd cmd ;
cmd.profile = "linear" ;
cmd.period = 1.0 ;
cmd.amplitude = 0.0 ;
@@ -252,7 +252,7 @@
return true;
}
-bool LaserScannerTrajController::setPeriodicCmd(const pr2_mechanism_controllers::PeriodicCmd& cmd)
+bool LaserScannerTrajController::setPeriodicCmd(const pr2_msgs::PeriodicCmd& cmd)
{
if (cmd.profile == "linear" ||
cmd.profile == "blended_linear")
@@ -422,8 +422,8 @@
return true ;
}
-bool LaserScannerTrajControllerNode::setPeriodicSrv(pr2_mechanism_controllers::SetPeriodicCmd::Request &req,
- pr2_mechanism_controllers::SetPeriodicCmd::Response &res)
+bool LaserScannerTrajControllerNode::setPeriodicSrv(pr2_srvs::SetPeriodicCmd::Request &req,
+ pr2_srvs::SetPeriodicCmd::Response &res)
{
ROS_INFO("LaserScannerTrajControllerNode: set periodic command");
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_velocity_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_velocity_controller.cpp 2009-04-30 23:56:37 UTC (rev 14703)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_velocity_controller.cpp 2009-04-30 23:58:54 UTC (rev 14704)
@@ -1,368 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2008, 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.
- *********************************************************************/
-#include <algorithm>
-
-#include <pr2_mechanism_controllers/laser_scanner_velocity_controller.h>
-
-using namespace std;
-using namespace controller;
-#define MIN_MOVING_SPEED 0.3
-ROS_REGISTER_CONTROLLER(LaserScannerVelocityController)
-
-LaserScannerVelocityController::LaserScannerVelocityController()
-{
- robot_ = NULL;
- joint_ = NULL;
-
- command_ = 0;
- last_time_ = 0;
- current_mode_ = VELOCITY; //Start out in velocity mode
-
-
- passed_center_ = true;
- upper_turnaround_offset_ = 0.0;
- lower_turnaround_offset_ = 0.0;
- automatic_turnaround_ = false;
- upper_deceleration_buffer_ = 0.0;
- lower_deceleration_buffer_ = 0.0;
-}
-
-LaserScannerVelocityController::~LaserScannerVelocityController()
-{
-
-}
-
-void LaserScannerVelocityController::init(double p_gain, double i_gain, double d_gain, double windup, double time, std::string name, mechanism::RobotState *robot)
-{
- robot_ = robot;
- joint_ = robot->getJointState(name);
-
- abort(); //joint_position_controller_.init( p_gain, i_gain, d_gain, windup, time, name, robot);
- command_= 0;
- last_time_= time;
-}
-
-bool LaserScannerVelocityController::initXml(mechanism::RobotState *robot, TiXmlElement *config)
-{
- assert(robot);
-
-
- robot_ = robot;
- last_time_ = robot->hw_->current_time_;
-
- TiXmlElement *vel = config->FirstChildElement("velocity");
-
- //Perform checks at highest level to give the most informative error message possible
- TiXmlElement *j = vel->FirstChildElement("joint");
- if (!j)
- {
- fprintf(stderr, "LaserScannerController was not given a joint\n");
- return false;
- }
-
- const char *joint_name = j->Attribute("name");
- int index = joint_name ? robot->model_->getJointIndex(joint_name) : -1;
-
- if (index < 0)
- {
- fprintf(stderr, "JointPositionController could not find joint named \"%s\"\n", joint_name);
- return false;
- }
- joint_ = &robot->joint_states_[index];
-
- TiXmlElement *p = j->FirstChildElement("pid");
- if (!p)
- fprintf(stderr, "LaserScannerController's config did not specify the default pid parameters.\n");
-
- joint_velocity_controller_.initXml(robot,vel); //Pass down XML snippet to encapsulated joint_velocity_controller_
-
- TiXmlElement *pos = config->FirstChildElement("position");
-
- joint_position_controller_.initXml(robot,pos); //Pass down XML snippet to encapsulated joint_position_controller_
-
- last_position_ = joint_->position_;
- return true;
- }
-
-// Set the joint velocity command
-void LaserScannerVelocityController::setCommand(double command)
-{
- command_ = command;
- if(current_mode_ == VELOCITY && automatic_turnaround_)
- {
- if(joint_->position_> upper_turnaround_location_)
- {
- if(command_ > 0) command_ = -command_; //Turn command around to avoid crash
- passed_center_ = false; //Allow passing through
- }
- else if (joint_->position_ < lower_turnaround_location_)
- {
- if(command_<0) command_ = -command_;
- passed_center_ = false;
- }
- }
-}
-
-// Return the current position command
-double LaserScannerVelocityController::getCommand()
-{
- return command_;
-}
-
-// Return the measured joint position
-double LaserScannerVelocityController::getMeasuredPosition()
-{
- return joint_->position_;
-}
-
-double LaserScannerVelocityController::getTime()
-{
- return robot_->hw_->current_time_;
-}
-
-void LaserScannerVelocityController::update()
-{
- double time = robot_->hw_->current_time_;
- double position = joint_->position_;
- double truncated_command, ratio;
-
- switch(current_mode_)
- {
- case VELOCITY:
-
- joint_velocity_controller_.setCommand(command_);
-
- if(automatic_turnaround_) //Automatically turn around
- {
- //Track if we passed center point
- if((position>=0 && last_position_<=0) || (position<=0 && last_position_>=0)) passed_center_ = true;
-
- if(passed_center_) //Check whether we're moving towards the edge
- {
-
- if(position >= upper_deceleration_zone_)
- {
- //Scale command for deceleration
- ratio = (1-(position-upper_deceleration_zone_)/(upper_turnaround_location_-upper_deceleration_zone_));
- truncated_command = command_* ratio ;
- truncated_command = max(MIN_MOVING_SPEED,truncated_command);
- joint_velocity_controller_.setCommand(truncated_command);
- }
- else if (position<=lower_deceleration_zone_)
- {
- ratio = (1-(lower_deceleration_zone_ - position)/(lower_deceleration_zone_-lower_turnaround_location_));
- truncated_command = command_* ratio;
- truncated_command = min(-MIN_MOVING_SPEED,truncated_command);
- joint_velocity_controller_.setCommand(truncated_command);
- }
-
- if( position>= upper_turnaround_location_ ||
- position<=lower_turnaround_location_ )
- {
- command_ = -command_; //Reverse direction
- joint_velocity_controller_.setCommand(0);
- passed_center_ = false; //Make sure we don't get stuck on edges of workspace
- }
- }
- else
- {
-
- if(position >= upper_deceleration_zone_)
- {
- ratio = (1-(position-upper_deceleration_zone_)/(upper_turnaround_location_-upper_deceleration_zone_));
- truncated_command = command_* ratio;
- truncated_command = min(-MIN_MOVING_SPEED,truncated_command);
- joint_velocity_controller_.setCommand(truncated_command);
- }
- else if (position<=lower_deceleration_zone_)
- {
- ratio = (1-(lower_deceleration_zone_ - position)/(lower_deceleration_zone_-lower_turnaround_location_));
- truncated_command = command_* ratio;
- truncated_command = max(MIN_MOVING_SPEED,truncated_command);
- joint_velocity_controller_.setCommand(truncated_command);
- }
-
- }
- }
-
- joint_velocity_controller_.update(); //Update lower controller
- break;
- case POSITION:
- joint_position_controller_.setCommand(command_);
- joint_position_controller_.update();
- break;
- default:
- break;
- }
-
- last_time_ = time; //Keep track of last time for update
- last_position_ = position;
-
-}
-
-void LaserScannerVelocityController::setJointEffort(double effort)
-{
- joint_->commanded_effort_ = effort;
-}
-
-void LaserScannerVelocityController::setTurnaroundPoints(void)
-{
- upper_turnaround_location_ = joint_->joint_->joint_limit_max_ - upper_turnaround_offset_;
- upper_deceleration_zone_ = upper_turnaround_location_-upper_deceleration_buffer_;
-
- lower_turnaround_location_ = joint_->joint_->joint_limit_min_ + lower_turnaround_offset_;
- lower_deceleration_zone_ = lower_turnaround_location_ + lower_deceleration_buffer_;
-}
-
-ROS_REGISTER_CONTROLLER(LaserScannerVelocityControllerNode)
-LaserScannerVelocityControllerNode::LaserScannerVelocityControllerNode()
-{
- c_ = new LaserScannerVelocityController();
-}
-
-
-LaserScannerVelocityControllerNode::~LaserScannerVelocityControllerNode()
-{
- delete c_;
-}
-
-void LaserScannerVelocityControllerNode::update()
-{
- c_->update();
-}
-
-// Return the measured joint position
-double LaserScannerVelocityControllerNode::getMeasuredPosition()
-{
- return c_->getMeasuredPosition();
-}
-
-bool LaserScannerVelocityControllerNode::setCommand(
- robot_mechanism_controllers::SetCommand::Request &req,
- robot_mechanism_controllers::SetCommand::Response &resp)
-{
-
- c_->current_mode_ = LaserScannerVelocityController::VELOCITY;
- c_->setCommand(req.command);
- resp.command = c_->getCommand();
-
- return true;
-}
-
-bool LaserScannerVelocityControllerNode::getCommand(
- robot_mechanism_controllers::GetCommand::Request &req,
- robot_mechanism_controllers::GetCommand::Response &resp)
-{
- resp.command = c_->getCommand();
-
- return true;
-}
-
-bool LaserScannerVelocityControllerNode::setPosition(
- robot_mechanism_controllers::SetPosition::Request &req,
- robot_mechanism_controllers::SetPosition::Response &resp)
-{
-
- c_->current_mode_ = LaserScannerVelocityController::POSITION;
- c_->setCommand(req.position);
- resp.command = c_->getCommand();
-
- return true;
-}
-
-bool LaserScannerVelocityControllerNode::getPosition(
- robot_mechanism_controllers::GetPosition::Request &req,
- robot_mechanism_controllers::GetPosition::Response &resp)
-{
- c_->current_mode_ = LaserScannerVelocityController::POSITION;
- resp.command = c_->getCommand();
-
- return true;
-}
-
-
-bool LaserScannerVelocityControllerNode::setProfile(
- pr2_mechanism_controllers::SetProfile::Request &req,
- pr2_mechanism_controllers::SetProfile::Response &resp)
-{
- setProfileCall(req.UpperTurnaround,req.LowerTurnaround, req.upperDecelBuffer, req.lowerDecelBuffer);
- resp.time = c_->getTime();
- return true;
-}
-
-bool LaserScannerVelocityControllerNode::initXml(mechanism::RobotState *robot, TiXmlElement *config)
-{
- ros::Node *node = ros::Node::instance();
- string prefix = config->Attribute("name");
-
- if (!c_->initXml(robot, config))
- return false;
- node->advertiseService(prefix + "/set_command", &LaserScannerVelocityControllerNode::setCommand, this);
- node->advertiseService(prefix + "/get_command", &LaserScannerVelocityControllerNode::getCommand, this);
- node->advertiseService(prefix + "/set_profile", &LaserScannerVelocityControllerNode::setProfile, this);
- node->advertiseService(prefix + "/set_position", &LaserScannerVelocityControllerNode::setPosition, this);
- node->advertiseService(prefix + "/get_position", &LaserScannerVelocityControllerNode::getPosition, this);
- return true;
-}
-
-bool LaserScannerVelocityControllerNode::setProfileCall(double upper_turn_around, double lower_turn_around, double upper_decel_buffer, double lower_decel_buffer)
-{
- c_->upper_turnaround_offset_ = upper_turn_around;
- c_->lower_turnaround_offset_ = lower_turn_around;
- c_->lower_deceleration_buffer_ = upper_decel_buffer;
- c_->upper_deceleration_buffer_ = lower_decel_buffer;
- c_->setTurnaroundPoints();
-
- if(upper_turn_around == 0 && lower_turn_around ==0) c_->automatic_turnaround_ = false;
- else c_->automatic_turnaround_ = true;
- c_->current_mode_ = LaserScannerVelocityController::VELOCITY;
- c_->setCommand(0.0);
-
- return true;
-}
-
-
-void LaserScannerVelocityControllerNode::setCommand(double command)
-{
- c_->setCommand(command);
-}
-
-// Return the current position command
-double LaserScannerVelocityControllerNode::getCommand()
-{
- return c_->getCommand();
-}
-
-
-
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetPeriodicCmd.srv
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetPeriodicCmd.srv 2009-04-30 23:56:37 UTC (rev 14703)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetPeriodicCmd.srv 2009-04-30 23:58:54 UTC (rev 14704)
@@ -1,3 +0,0 @@
-PeriodicCmd command
----
-time start_time
\ No newline at end of file
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|