Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[22196] pkg/trunk
|
From: <stu...@us...> - 2009-08-19 00:43:08
|
Revision: 22196
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22196&view=rev
Author: stuglaser
Date: 2009-08-19 00:42:57 +0000 (Wed, 19 Aug 2009)
Log Message:
-----------
Moved controllers that are not set for M3 release out of pr2_mechanism_controllers
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
pkg/trunk/controllers/pr2_mechanism_controllers/controller_plugins.xml
pkg/trunk/controllers/pr2_mechanism_controllers/src/controller_manifest.cpp
pkg/trunk/sandbox/experimental_controllers/CMakeLists.txt
pkg/trunk/sandbox/experimental_controllers/controller_plugins.xml
pkg/trunk/sandbox/experimental_controllers/manifest.xml
pkg/trunk/sandbox/experimental_controllers/src/controller_manifest.cpp
Added Paths:
-----------
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/arm_trajectory_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/base_position_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/base_trajectory_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/caster_calibration_controller_effort.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/caster_controller_effort.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/head_servoing_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_trajectory_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/laser_scanner_qualification.h
pkg/trunk/sandbox/experimental_controllers/src/arm_trajectory_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/base_position_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/base_trajectory_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/caster_calibration_controller_effort.cpp
pkg/trunk/sandbox/experimental_controllers/src/caster_controller_effort.cpp
pkg/trunk/sandbox/experimental_controllers/src/head_servoing_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_trajectory_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/laser_scanner_qualification.cpp
Removed Paths:
-------------
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_position_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_trajectory_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_calibration_controller_effort.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_controller_effort.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_servoing_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/joint_trajectory_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_qualification.h
pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/base_position_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/base_trajectory_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_calibration_controller_effort.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_controller_effort.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/head_servoing_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/joint_trajectory_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_qualification.cpp
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt 2009-08-19 00:41:27 UTC (rev 22195)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt 2009-08-19 00:42:57 UTC (rev 22196)
@@ -14,15 +14,12 @@
src/controller_manifest.cpp
src/laser_scanner_traj_controller.cpp
src/head_position_controller.cpp
- src/head_servoing_controller.cpp
src/caster_controller.cpp
#src/caster_controller_effort.cpp
src/caster_calibration_controller.cpp
#src/caster_calibration_controller_effort.cpp
src/wrist_calibration_controller.cpp
src/gripper_calibration_controller.cpp
- src/arm_trajectory_controller.cpp
- src/joint_trajectory_controller.cpp
src/base_kinematics.cpp
#src/pr2_gripper_controller.cpp
src/pr2_odometry.cpp
@@ -31,6 +28,3 @@
rospack_link_boost(pr2_mechanism_controllers thread)
rospack_remove_compile_flags(pr2_mechanism_controllers -W)
-
-rospack_add_executable(base_trajectory_controller src/base_trajectory_controller.cpp)
-rospack_declare_test(base_trajectory)
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/controller_plugins.xml
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/controller_plugins.xml 2009-08-19 00:41:27 UTC (rev 22195)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/controller_plugins.xml 2009-08-19 00:42:57 UTC (rev 22196)
@@ -5,12 +5,9 @@
<plugin name="GripperCalibrationController" class="GripperCalibrationController" type="Controller" />
<plugin name="GripperCalibrationControllerNode" class="GripperCalibrationControllerNode" type="Controller" />
<plugin name="HeadPositionController" class="HeadPositionController" type="Controller" />
- <plugin name="HeadServoingController" class="HeadServoingController" type="Controller" />
- <plugin name="JointTrajectoryController" class="JointTrajectoryController" type="Controller" />
<plugin name="LaserScannerTrajController" class="LaserScannerTrajController" type="Controller" />
<plugin name="LaserScannerTrajControllerNode" class="LaserScannerTrajControllerNode" type="Controller" />
<plugin name="Pr2BaseController" class="Pr2BaseController" type="Controller" />
<plugin name="Pr2Odometry" class="Pr2Odometry" type="Controller" />
<plugin name="WristCalibrationController" class="WristCalibrationController" type="Controller" />
- <plugin name="ArmTrajectoryControllerNode" class="ArmTrajectoryControllerNode" type="Controller" />
</library>
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h 2009-08-19 00:41:27 UTC (rev 22195)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h 2009-08-19 00:42:57 UTC (rev 22196)
@@ -1,312 +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 <boost/thread/mutex.hpp>
-
-#include <controller_interface/controller.h>
-#include <robot_mechanism_controllers/joint_position_controller.h>
-#include <robot_mechanism_controllers/joint_velocity_controller.h>
-#include <robot_mechanism_controllers/joint_effort_controller.h>
-#include <robot_mechanism_controllers/joint_pd_controller.h>
-
-// Services
-#include <pr2_mechanism_controllers/SetJointPosCmd.h>
-#include <pr2_mechanism_controllers/GetJointPosCmd.h>
-
-#include <pr2_mechanism_controllers/SetJointGains.h>
-#include <pr2_mechanism_controllers/GetJointGains.h>
-
-#include <pr2_mechanism_controllers/SetCartesianPosCmd.h>
-#include <pr2_mechanism_controllers/GetCartesianPosCmd.h>
-
-#include <pr2_mechanism_controllers/SetJointTarget.h>
-#include <pr2_mechanism_controllers/JointPosCmd.h>
-
-#include <manipulation_msgs/JointTraj.h>
-#include <manipulation_msgs/JointTrajPoint.h>
-#include <diagnostic_msgs/DiagnosticArray.h>
-
-#include <pr2_mechanism_controllers/TrajectoryStart.h>
-#include <pr2_mechanism_controllers/TrajectoryQuery.h>
-#include <pr2_mechanism_controllers/TrajectoryCancel.h>
-
-#include <trajectory/trajectory.h>
-
-#include <pr2_mechanism_controllers/ControllerState.h>
-#include <realtime_tools/realtime_publisher.h>
-#include <std_msgs/String.h>
-
-namespace controller
-{
-
- #define GOAL_REACHED_THRESHOLD 0.01
-
-
- // comment this out if the controller is not supposed to publish its own max execution time
- #define PUBLISH_MAX_TIME
-
-
-// The maximum number of joints expected in an arm.
- static const int MAX_ARM_JOINTS = 7;
-
- class ArmTrajectoryController : public Controller
- {
- public:
-
- /*!
- * \brief Default Constructor of the JointController class.
- *
- */
- ArmTrajectoryController();
-
- /*!
- * \brief Destructor of the JointController class.
- */
- virtual ~ArmTrajectoryController();
-
- /*!
- * \brief Functional way to initialize limits and gains.
- */
- bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
-
- /*!
- * \brief set the joint trajectory for the arm
- */
- void setTrajectoryCmd(const std::vector<trajectory::Trajectory::TPoint>& joint_trajectory);
-
- void getJointPosCmd(pr2_mechanism_controllers::JointPosCmd & cmd) const;
-
- /*!
- * \brief Issues commands to the joint. Should be called at regular intervals
- */
- virtual void update(void); // Real time safe.
-
- boost::mutex arm_controller_lock_;
-
- controller::JointPDController* getJointControllerByName(std::string name);
-
- private:
-
- std::vector<JointPDController *> joint_pd_controllers_;
-
- trajectory::Trajectory *joint_trajectory_;
-
- trajectory::Trajectory::TPoint trajectory_point_;
-
- std::vector<double> joint_cmd_rt_;
-
- std::vector<double> joint_cmd_dot_rt_;
-
- std::vector<int> joint_type_;
-
- mechanism::Robot* robot_;
-
- void updateJointControllers(void);
-
- bool reachedGoalPosition(std::vector<double> joint_cmd);
-
- int getJointControllerPosByName(std::string name);
-
- // Indicates if goals_ and error_margins_ should be copied into goals_rt_ and error_margins_rt_
- bool refresh_rt_vals_;
-
- double trajectory_start_time_;
-
- double trajectory_end_time_;
-
- double current_time_;
-
- bool trajectory_done_;
-
- int dimension_;
-
- std::vector<double> joint_velocity_limits_;
-
- std::string trajectory_type_;
-
- double velocity_scaling_factor_;
-
- friend class ArmTrajectoryControllerNode;
-
- double trajectory_wait_time_;
-
- std::vector<double> goal_reached_threshold_;
-
- realtime_tools::RealtimePublisher <pr2_mechanism_controllers::ControllerState>* controller_state_publisher_ ; //!< Publishes controller information
-
-
- double max_update_time_;
- };
-
-/** @class ArmTrajectoryControllerNode
- * @brief ROS interface for the arm controller.
- * @author Sachin Chitta <sa...@wi...>
- *
- * This class provides a ROS interface for controlling the arm by setting position configurations. If offers several ways to control the arms:
- * - through listening to ROS messages: this is specified in the XML configuration file by the following parameters:
- * <listen_topic name="the name of my message" />
- * (only one topic can be specified)
- * - through a non blocking service call: this service call can specify a single configuration as a target (and maybe multiple configuration in the future)
- * - through a blocking service call: this service can receive a list of position commands that will be followed one after the other
- * @note This controller makes the assumptiom that a point update is real-time safe.
- * This is not the case for example for the LQR controller.
- *
- */
- class ArmTrajectoryControllerNode : public Controller
- {
- public:
- /*!
- * \brief Default Constructor
- *
- */
- ArmTrajectoryControllerNode();
-
- /*!
- * \brief Destructor
- */
- ~ArmTrajectoryControllerNode();
-
- void update();
-
- bool init(mechanism::RobotState *robot, const ros::NodeHandle& n);
- bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
-
- /** @brief service that returns the goal of the controller
- * @note if you know the goal has been reached and you do not want to subscribe to the /mechanism_state topic, you can use it as a hack to get the position of the arm
- * @param req
- * @param resp the response, contains a JointPosCmd message with the goal of the controller
- * @return
- */
- bool getJointPosCmd(pr2_mechanism_controllers::GetJointPosCmd::Request &req,
- pr2_mechanism_controllers::GetJointPosCmd::Response &resp);
-
- bool setJointTrajSrv(pr2_mechanism_controllers::TrajectoryStart::Request &req,
- pr2_mechanism_controllers::TrajectoryStart::Response &resp);
-
- bool queryJointTrajSrv(pr2_mechanism_controllers::TrajectoryQuery::Request &req,
- pr2_mechanism_controllers::TrajectoryQuery::Response &resp);
-
- bool cancelJointTrajSrv(pr2_mechanism_controllers::TrajectoryCancel::Request &req,
- pr2_mechanism_controllers::TrajectoryCancel::Response &resp);
-
- void deleteTrajectoryFromQueue(int id);
-
- void addTrajectoryToQueue(manipulation_msgs::JointTraj new_traj, int id);
-
- int createTrajectory(const manipulation_msgs::JointTraj &new_traj,trajectory::Trajectory &return_trajectory);
-
- void updateTrajectoryQueue(int last_trajectory_finish_status);
-
- void getJointTrajectoryThresholds();
-
- private:
-
- void publishDiagnostics();
-
- realtime_tools::RealtimePublisher <diagnostic_msgs::DiagnosticArray>* diagnostics_publisher_ ; //!< Publishes controller information
-
- /*!
- * \brief mutex lock for setting and getting ros messages
- */
- boost::mutex ros_lock_;
-
- manipulation_msgs::JointTraj traj_msg_;
-
- pr2_mechanism_controllers::JointPosCmd msg_; //The message used by the ROS callback
- ArmTrajectoryController *c_;
-
- /*!
- * \brief service prefix
- */
- std::string service_prefix_;
-
- /*
- * \brief save topic name for unsubscribe later
- */
- std::string topic_name_;
-
- /*!
- * \brief xml pointer to ros topic name
- */
- TiXmlElement * topic_name_ptr_;
-
- /*
- * \brief pointer to ros node
- */
- ros::Node * const node_;
-
-
- /*
- * \brief receive and set the trajectory in the controller
- */
- void CmdTrajectoryReceived();
-
- int trajectory_id_;
-
- std::vector<manipulation_msgs::JointTraj> joint_trajectory_vector_;
-
- std::vector<int> joint_trajectory_id_;
-
- void setTrajectoryCmdFromMsg(manipulation_msgs::JointTraj traj_msg);
-
- int request_trajectory_id_;
-
- int current_trajectory_id_;
-
- std::map<int,int> joint_trajectory_status_;
-
- std::map<int,double>joint_trajectory_time_;
-
- enum JointTrajectoryStatus{
- ACTIVE,
- DONE,
- QUEUED,
- DELETED,
- FAILED,
- CANCELED,
- NUM_STATUS
- };
-
- double trajectory_wait_timeout_;
-
- double last_diagnostics_publish_time_;
-
- double diagnostics_publish_delta_time_;
-
- };
-
-}
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_position_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_position_controller.h 2009-08-19 00:41:27 UTC (rev 22195)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_position_controller.h 2009-08-19 00:42:57 UTC (rev 22196)
@@ -1,115 +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.
- *********************************************************************/
-
-#ifndef PR2_MECHANISM_CONTROLLERS_BASE_POSITION_CONTROLLER_H
-#define PR2_MECHANISM_CONTROLLERS_BASE_POSITION_CONTROLLER_H
-
-#include "control_toolbox/base_position_pid.h"
-#include "pr2_mechanism_controllers/base_controller.h"
-#include "tf/transform_listener.h"
-#include "geometry_msgs/PoseStamped.h"
-#include "geometry_msgs/Point.h"
-
-#include "misc_utils/advertised_service_guard.h"
-
-namespace pr2_mechanism_controllers
-{
-
-class BasePositionControllerNode : public controller::Controller
-{
-public :
- BasePositionControllerNode() ;
- ~BasePositionControllerNode() ;
-
- /**
- * Initializes the Controller.
- * A list of XML sections are necessary for this to work
- *
- *
- *
- *
- */
- bool initXml(mechanism::RobotState *robot_state, TiXmlElement *config) ;
-
- /**
- * Realtime safe update method called from the realtime loop. Compares the target position to the current position,
- * generates error terms, and then uses these to generate a Velocity command for BaseController
- */
- void update() ;
-
- /**
- * Sets the target pose of the controller. The cmd is transformed and stored into the odometric frame using the latest transform. The x,y position
- * of the pose define the target location of the robot (in the odometric frame). The theta rotation of the robot is defined by the pure z rotation that is
- * closest to the orientation of the pose in the odometric frame.
- * \param cmd The pose that we want to reach
- */
- void setPoseCommand(geometry_msgs::PoseStamped cmd) ;
-
- /**
- * Sets an x,y,theta position for the base to reach in wheel odometry frame. This command doesn't do any transforms. It simply sets the PID targets
- * for the 3 different axes of the base.
- * \param x The x position that we want to reach [in the odometric frame]
- * \param y The y position that we want to reach [in the odometric frame]
- * \param w The theta angle that we want to reach [in the odometric frame]
- */
- void setPoseOdomFrameCommand(double x, double y, double w) ;
-
-private :
- control_toolbox::BasePositionPid base_position_pid_ ; // Does the math to compute a command velocity
- controller::BaseControllerNode base_controller_node_ ; // Converts a commanded velocity into a wheel velocities and turret angles
- ros::Node *node_ ;
-
- mechanism::RobotState *robot_state_ ;
-
- double last_time_ ; // Store the last time that we called the update
-
- SubscriptionGuard guard_set_pose_command_ ; // Automatically unsubscribes set_pose_cmd
- void setPoseCommandCallback() ; // Ros callback for "set_pose_command" messages
-
- SubscriptionGuard guard_set_pose_odom_frame_command_ ; // Automatically unsubscribes set_pose_odom_frame_cmd
- void setPoseOdomFrameCommandCallback() ; // Ros callback for "set_pose_command_odom_frame" messages
-
- tf::Vector3 xyt_target_ ; // The current x,y,theta target in position space (NOT velocity space)
-
- tf::TransformListener tf_ ; // Transformer used to convert commands from native frame into odometric frame
- std::string odom_frame_name_ ; // Stores the name of the odometric frame. This is the frame that we control in
-
- // Message Holders
- geometry_msgs::PoseStamped pose_cmd_ ;
- geometry_msgs::Point pose_odom_frame_cmd_ ;
-} ;
-
-}
-
-#endif // PR2_MECHANISM_CONTROLLERS_BASE_POSITION_CONTROLLER_H
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_trajectory_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_trajectory_controller.h 2009-08-19 00:41:27 UTC (rev 22195)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_trajectory_controller.h 2009-08-19 00:42:57 UTC (rev 22196)
@@ -1,129 +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.
-*
-* Author: Sachin Chitta
-*********************************************************************/
-
-#include <geometry_msgs/Twist.h>
-#include <pr2_robot_actions/Pose2D.h>
-#include <manipulation_msgs/JointTrajPoint.h>
-#include <manipulation_msgs/JointTraj.h>
-#include <trajectory/trajectory.h>
-
-#include <tf/tf.h>
-#include <tf/transform_listener.h>
-#include <control_toolbox/pid.h>
-
-#include <angles/angles.h>
-
-namespace pr2_mechanism_controllers
-{
- class BaseTrajectoryController
- {
- public:
-
- BaseTrajectoryController(ros::Node& node, tf::TransformListener& tf);
-
- ~BaseTrajectoryController();
-
- trajectory::Trajectory::TPoint getPose2D(const tf::Stamped<tf::Pose> &pose);
-
- void pathCallback();
-
- void updateGlobalPose();
-
- double distance(double x1, double y1, double x2, double y2);
-
- bool goalPositionReached();
-
- bool goalOrientationReached();
-
- bool goalReached();
-
- void updatePath();
-
- void spin();
-
- void updateControl();
-
- geometry_msgs::Twist getCommand();
-
- private:
- tf::Stamped<tf::Pose> global_pose_;
- std::vector<control_toolbox::Pid> pid_;
- std::string global_frame_, robot_base_frame_;
- double controller_frequency_;
- std::string control_topic_name_, path_input_topic_name_;
- std::string trajectory_type_;
- manipulation_msgs::JointTraj path_msg_in_;
- manipulation_msgs::JointTraj path_msg_;
-
- ros::Node& ros_node_;
- tf::TransformListener& tf_;
- int dimension_;
- double current_time_;
- double sample_time_;
- double last_update_time_;
- double trajectory_start_time_;
- trajectory::Trajectory *trajectory_;
- bool stop_motion_;
- int stop_motion_count_;
- bool new_path_available_;
-
- std::vector<double> velocity_limits_;
-
- trajectory::Trajectory::TPoint goal_;
- trajectory::Trajectory::TPoint current_position_;
- double yaw_goal_tolerance_;
- double xy_goal_tolerance_;
-
- boost::mutex ros_lock_;
- double path_updated_time_;
- double max_update_time_;
-
- geometry_msgs::Twist checkCmd(const geometry_msgs::Twist &cmd);
-
- double diagnostics_expected_publish_time_;
-
- ros::Time last_diagnostics_publish_time_;
-
- void publishDiagnostics(bool force);
-
- geometry_msgs::Twist cmd_vel_;
- double error_x_,error_y_,error_th_;
-
- std::string control_state_;
- };
-};
-
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_calibration_controller_effort.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_calibration_controller_effort.h 2009-08-19 00:41:27 UTC (rev 22195)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_calibration_controller_effort.h 2009-08-19 00:42:57 UTC (rev 22196)
@@ -1,105 +0,0 @@
-/*
- * 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, Inc. 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.
- */
-
-/*
- * Author: Wim Meeussen
- */
-
-#ifndef CASTER_CALIBRATION_CONTROLLER_EFFORT_H
-#define CASTER_CALIBRATION_CONTROLLER_EFFORT_H
-
-#include "pr2_mechanism_controllers/caster_controller_effort.h"
-#include "realtime_tools/realtime_publisher.h"
-#include "std_msgs/Empty.h"
-#include <robot_mechanism_controllers/CalibrateJoint.h>
-
-namespace controller {
-
-class CasterCalibrationControllerEffort : public Controller
-{
-public:
- CasterCalibrationControllerEffort();
- ~CasterCalibrationControllerEffort();
-
- virtual bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
-
-
- /*!
- * \brief Issues commands to the joint. Should be called at regular intervals
- */
- virtual void update();
-
- bool calibrated() { return state_ == CALIBRATED; }
- void beginCalibration()
- {
- if (state_ == INITIALIZED)
- state_ = BEGINNING;
- }
-
-protected:
-
- enum { INITIALIZED, BEGINNING, MOVING, CALIBRATED };
- int state_;
-
- double search_velocity_;
- bool original_switch_state_;
-
- Actuator *actuator_;
- mechanism::JointState *joint_;
- mechanism::Transmission *transmission_;
-
- controller::CasterControllerEffort cc_;
-};
-
-
-class CasterCalibrationControllerEffortNode : public Controller
-{
-public:
- CasterCalibrationControllerEffortNode();
- ~CasterCalibrationControllerEffortNode();
-
- void update();
-
- bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
-
- bool calibrateCommand(robot_mechanism_controllers::CalibrateJoint::Request &req,
- robot_mechanism_controllers::CalibrateJoint::Response &resp);
-
-private:
- mechanism::RobotState *robot_;
- CasterCalibrationControllerEffort c_;
- AdvertisedServiceGuard guard_calibrate_;
-
- double last_publish_time_;
- realtime_tools::RealtimePublisher<std_msgs::Empty> *pub_calibrated_;
-};
-
-}
-
-#endif
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_controller_effort.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_controller_effort.h 2009-08-19 00:41:27 UTC (rev 22195)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_controller_effort.h 2009-08-19 00:42:57 UTC (rev 22196)
@@ -1,116 +0,0 @@
-/*
- * 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, Inc. 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.
- */
-
-/*
- * Author: Wim Meeussen
-
- Example config:
-
- <controller type="CasterControllerEffort" name="a_caster">
- <joints caster="caster_joint" wheel_l="wheel_left_joint" wheel_r="wheel_right_joint">
- <caster_pid p="5.0" i="0.0" d="0.0" iClamp="0.0" />
- <wheel_pid p="4.0" i="0.0" d="0.0" iClamp="0.0" />
- </controller>
- */
-#ifndef CASTER_CONTROLLER_EFFORT_H
-#define CASTER_CONTROLLER_EFFORT_H
-
-#include "controller_interface/controller.h"
-#include "mechanism_model/robot.h"
-#include "control_toolbox/pid.h"
-#include "robot_mechanism_controllers/joint_velocity_controller.h"
-#include "robot_mechanism_controllers/joint_effort_controller.h"
-#include "misc_utils/subscription_guard.h"
-#include "std_msgs/Float64.h"
-
-namespace controller {
-
-class CasterControllerEffort : public Controller
-{
-public:
- const static double WHEEL_RADIUS;
- const static double WHEEL_OFFSET;
- const static double VEL_TO_EFF;
-
- CasterControllerEffort();
- ~CasterControllerEffort();
-
- bool init(mechanism::RobotState *robot_state,
- const std::string &caster_joint,
- const std::string &wheel_l_joint, const std::string &wheel_r_joint,
- const control_toolbox::Pid &caster_pid);
- bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
-
- void update();
-
- double steer_velocity_;
- double drive_effort_;
-
- double getSteerPosition() { return caster_->position_; }
- double getSteerVelocity() { return caster_->velocity_; }
-
- mechanism::JointState *caster_;
-
-private:
- JointVelocityController caster_vel_;
- JointEffortController wheel_l_eff_, wheel_r_eff_;
-};
-
-
-/*
- * Listens on /name/steer_velocity and /name/drive_velocity
- */
-
-class CasterControllerEffortNode : public Controller
-{
-public:
- CasterControllerEffortNode();
- ~CasterControllerEffortNode();
-
- bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
-
- void update();
-
- void setSteerVelocity() {
- c_.steer_velocity_ = steer_velocity_msg_.data;
- }
- void setDriveEffort() {
- c_.drive_effort_ = drive_effort_msg_.data;
- }
-
-private:
- CasterControllerEffort c_;
-
- SubscriptionGuard guard_steer_velocity_, guard_drive_effort_;
- std_msgs::Float64 steer_velocity_msg_, drive_effort_msg_;
-};
-
-}
-
-#endif
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_servoing_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_servoing_controller.h 2009-08-19 00:41:27 UTC (rev 22195)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_servoing_controller.h 2009-08-19 00:42:57 UTC (rev 22196)
@@ -1,100 +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.
- *********************************************************************/
-
-// Original version: Melonee Wise <mw...@wi...>
-
-#ifndef HEAD_SERVOING_CONTROLLER_H
-#define HEAD_SERVOING_CONTROLLER_H
-
-#include <ros/node.h>
-#include <ros/node_handle.h>
-#include <controller_interface/controller.h>
-#include <tf/transform_listener.h>
-#include <tf/message_notifier.h>
-#include <tf/transform_datatypes.h>
-#include <robot_mechanism_controllers/joint_velocity_controller.h>
-#include <mechanism_msgs/JointStates.h>
-#include <geometry_msgs/PointStamped.h>
-#include <boost/scoped_ptr.hpp>
-#include <math.h>
-
-
-namespace controller
-{
-
-class HeadServoingController : public Controller
-{
-public:
-
- HeadServoingController();
- ~HeadServoingController();
-
- bool init(mechanism::RobotState *robot, const ros::NodeHandle &n);
-
- bool starting();
- void update();
-
- // input of the controller
- double pan_out_, tilt_out_;
-
-private:
-
- mechanism::Robot* robot_;
- ros::NodeHandle node_;
- std::string pan_link_name_, tilt_link_name_;
- mechanism::RobotState *robot_state_;
-
- double max_velocity_;
- double servo_gain_;
- ros::Subscriber sub_command_;
-
-
- void command(const mechanism_msgs::JointStatesConstPtr& command_msg);
-
- void pointHead(const tf::MessageNotifier<geometry_msgs::PointStamped>::MessagePtr& point_msg);
- void pointFrameOnHead(const tf::MessageNotifier<geometry_msgs::PointStamped>::MessagePtr& point_msg);
-
-
-
- tf::TransformListener tf_;
- boost::scoped_ptr<tf::MessageNotifier<geometry_msgs::PointStamped> > point_head_notifier_;
- boost::scoped_ptr<tf::MessageNotifier<geometry_msgs::PointStamped> > point_frame_on_head_notifier_;
-
- // position controller
- JointVelocityController head_pan_controller_;
- JointVelocityController head_tilt_controller_;
-
-};
-}
-#endif
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/joint_trajectory_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/joint_trajectory_controller.h 2009-08-19 00:41:27 UTC (rev 22195)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/joint_trajectory_controller.h 2009-08-19 00:42:57 UTC (rev 22196)
@@ -1,493 +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 <boost/thread/mutex.hpp>
-
-#include <controller_interface/controller.h>
-#include <robot_mechanism_controllers/joint_pd_controller.h>
-
-// Services
-#include <manipulation_msgs/JointTraj.h>
-#include <manipulation_msgs/JointTrajPoint.h>
-#include <diagnostic_msgs/DiagnosticArray.h>
-
-#include <pr2_mechanism_controllers/TrajectoryStart.h>
-#include <pr2_mechanism_controllers/TrajectoryQuery.h>
-#include <pr2_mechanism_controllers/TrajectoryCancel.h>
-
-//Kinematics
-#include <trajectory/trajectory.h>
-#include <pr2_mechanism_controllers/ControllerState.h>
-
-#include <realtime_tools/realtime_publisher.h>
-#include <std_msgs/String.h>
-
-#include <angles/angles.h>
-
-namespace controller
-{
-
- const std::string JointTrajectoryStatusString[8] = {"0 - ACTIVE","1 - DONE","2 - QUEUED","3 - DELETED","4 - FAILED","5 - CANCELED","6 - DOES_NOT_EXIST","7 - NUM_STATUS"};
-
-#define GOAL_REACHED_THRESHOLD 0.2
-#define MAX_ALLOWABLE_JOINT_ERROR_THRESHOLD 0.2
- // comment this out if the controller is not supposed to publish its own max execution time
-#define PUBLISH_MAX_TIME
-
-
-/** @class JointTrajectoryController
- * @brief ROS interface for a joint trajectory controller.
- * @author Sachin Chitta <sa...@wi...>
- *
- * This class provides a ROS interface for controlling a set of joints by setting position configurations. If offers several ways to control the joints:
- * - through listening to ROS messages: this is specified in the XML configuration file by the following parameters:
- * <listen_topic name="the name of my message" />
- * (only one topic can be specified)
- * - through a non blocking service call: this service call can specify a single trajectory as a goal
- *
- */
- class JointTrajectoryController : public Controller
- {
- public:
-
- /**
- * @brief Default Constructor of the JointController class.
- *
- */
- JointTrajectoryController();
-
- /*!
- * \brief Destructor of the JointController class. Stops publishers and services.
- */
- virtual ~JointTrajectoryController();
-
- /*!
- * @brief Initialize the controller using an Xml configuration and a robot object.
- * @param robot pointer to a robot object passed in by all controllers.
- * @config TiXml configuration element
- */
- bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
-
- bool init(mechanism::RobotState *robot, const ros::NodeHandle& n);
-
- /*!
- * @brief Issues commands to the joints and is called at regular intervals in realtime. This function is required
- * to be realtime safe.
- */
- virtual void update(void);
-
- virtual bool starting(void);
-
- private:
-
- std::string prefix_; /**< prefix (initialized to controller name + "/") for all controller & ROS interaction */
-
- boost::mutex arm_controller_lock_; /**< Mutex lock for sharing information between services and realtime */
-
- double last_time_; /**< last time update was called */
-
- std::vector<std::string> joint_name_; /**< names of joints controlled by this controller */
-
- std::vector<int> joint_type_; /**< joint types of the joints controlled by this controller, these are derived from the mechanism class */
-
- std::vector<double> current_joint_position_; /**< internal storage for the current joint positions */
-
- std::vector<double> current_joint_velocity_; /**< internal storage for the current joint velocities */
-
- std::vector<double> joint_position_errors_; /**< internal storage for the joint position errors (actual - command)*/
-
- std::vector<double> joint_velocity_errors_; /**< internal storage for the joint velocity errors (actual - command)*/
-
- std::vector<double> max_allowable_joint_errors_; /**< max allowable joint errors. These are loaded through the ROS Param server. e.g. <param name="r_shoulder_pan_joint/joint_error_threshold" type="double" value="0.1"/>*/
-
- std::vector<JointPDController *> joint_pv_controllers_; /**< internal set of controllers used to control individual joints */
-
- trajectory::Trajectory *joint_trajectory_; /**< internal representation of the trajectory specified for all the joints */
-
- std::vector<double> joint_cmd_; /**< internal representation of the instantaneous position commands for each joint */
-
- std::vector<double> joint_cmd_dot_; /**< internal representation of the instantaneous velocity commands for each joint */
-
- mechanism::Robot* robot_; /**< Pointer to a robot object */
-
- mechanism::RobotState* robot_state_; /**< Pointer to a robot state object */
-
- bool refresh_rt_vals_; /**< Indicates that a new trajectory has been received. */
-
- double trajectory_start_time_; /**< Start time for the current trajectory */
-
- double trajectory_end_time_; /**< End time for the current trajectory */
-
- double current_time_; /**< Current time */
-
- int num_joints_; /**< Number of joints controlled by this controller */
-
- std::vector<double> joint_velocity_limits_; /**< Velocity limits for each joint. These are derived from the robot object created using a representation of the robot */
-
- std::string trajectory_type_; /**< The trajectory type used for control. It can be "linear", "cubic"*/
-
- double velocity_scaling_factor_; /**< Scaling factor for the velocity limits */
-
- double trajectory_wait_time_; /**< The amount of time that the controller waits after the expected completion time for a trajectory before declaring that a trajectory has failed */
-
- std::vector<double> goal_reached_threshold_; /**< Threshold within which the joints must be before goal is declared to have been reached */
-
- realtime_tools::RealtimePublisher <pr2_mechanism_controllers::ControllerState>* controller_state_publisher_ ; /**< Publishes controller information */
-
- double max_update_time_; /**< maximum time (over the complete history of the run) taken for the update loop of this controller*/
-
- double last_traj_req_time_; /**< Last time a trajectory command was received on a topic or service */
-
- double max_allowed_update_time_; /**< Safety behavior: Max allowed time between commands */
-
- bool watch_dog_active_;
-
- realtime_tools::RealtimePublisher <diagnostic_msgs::DiagnosticArray>* diagnostics_publisher_ ; /**< Publishes controller information as a diagnostic message */
-
- /*!
- * \brief mutex lock for setting and getting ros messages
- */
- boost::mutex ros_lock_;
-
- manipulation_msgs::JointTraj traj_msg_; /**< The trajectory message received over ROS */
-
- /*!
- * \brief node name
- */
- std::string name_;
-
- /*
- * \brief Topic on which the controller listens for commands
- */
- std::string listen_topic_name_;
-
- /*
- * \brief pointer to ros node
- */
- ros::Node * const node_;
-
- std::vector<manipulation_msgs::JointTraj> joint_trajectory_vector_; /**< Vector of trajectory requests */
-
- std::vector<int> joint_trajectory_id_; /**< Vector of ids for trajectory requests */
-
- /**
- * @brief Get the parameters from the ROS parameter server
- */
- void getParams();
-
- /**
- * @brief Initialize publishers (diagnostic, state)
- */
- void initializePublishers();
-
- /**
- * @brief Stop publishers (diagnostic, state)
- */
- void stopPublishers();
-
- /**
- * @brief Advertise all services
- */
- void advertiseServices();
-
- /**
- * @brief Unadvertise all services
- */
- void unadvertiseServices();
-
- /**
- * @brief Subscribe to topics
- */
- void subscribeTopics();
-
- /**
- * @brief Unsubscribe to topics
- */
- void unsubscribeTopics();
-
- /**
- * @brief Load parameters from an XML file
- * @param robot pointer to a robot object passed in by all controllers.
- * @config TiXml configuration element
- */
- bool loadXmlFile(mechanism::RobotState * robot, TiXmlElement * config);
-
- /**
- * @brief Initialize the trajectory from a tiny XML element
- */
- void initTrajectory(TiXmlElement * config);
-
- /**
- * @brief Set the trajectory command to the current values of each joint. This has the effect of stopping all joints
- * at their current positions
- */
- void setTrajectoryCmdToCurrentValues();
-
- /**
- * @brief Add a joint to the list of joints controlled by this controller.
- * @param name The name of the joint in the mechanism structure.
- */
- void addJoint(const std::string &name);
-
-
- /**
- * @brief Set the commanded trajectory for the controller
- * @param joint_trajectory A vector of trajectory points that this controller must follow
- */
- bool setTrajectoryCmd(const std::vector<trajectory::Trajectory::TPoint>& joint_trajectory);
-
- /**
- * @brief Determine if the joint position errors are within the threshold specified by goal_reached_threshold.
- * These thresholds are set using the ROS param server e.g. <param name="r_shoulder_pan_joint/goal_reached_threshold" type="double" value="0.1"/>
- * @return true if absolute position errors are less than threshold values, false otherwise
- */
- bool errorsWithinThreshold();
-
- /**
- * @brief Determine if the joints are at rest.
- * @return true if absolute velocities are less than zero
- */
- bool atRest();
-
- /**
- * @brief Compute the errors for each joint
- */
- void computeJointErrors();
-
- /**
- * @brief Check watchdog. If there has been no trajectory request externally for a while or if the position errors are too great, this will set the current position to the desired position, thus effectively stopping the robot.
- * The parameters that affect this function are
- * (a) max_allowed_update_time_ which can be set using a ROS param call, e.g. <param name="max_allowed_update_time" type="double" value="0.1"/>
- * (b) max_allowable_joint_errors_ which can be set individually for each joint using a ROS param call, e.g. <param name="r_shoulder_pan_joint/joint_error_threshold" type="double" value="0.1"/>
- */
- bool checkWatchDog(double current_time);
-
- /**
- * @brief Stop the motion of all joints. In addition to setting all desired joint positions to the current position, it also sets velocities for the base to zero
- */
- void stopMotion();
-
- /**
- * @brief Reset all the trajectory times on completion of an old trajectory or receipt of a new one that preempts the previous one
- */
- void resetTrajectoryTimes();
-
- /**
- * @brief Internal function to get the actual set point values from the trajectory.
- */
- void getJointCommands();
-
- /**
- * @brief Check if the trajectory is done. If done, pop the next trajectory from the queue and use it. If no new trajectory is available, set the desired joint positions to the current joint positions
- */
- bool trajectoryDone();
-
-
- /**
- * @brief Check if the goal position has been reached
- */
- bool reachedGoalPosition(std::vector<double> joint_cmd);
-
- /**
- * @brief Call update() on each joint controller. (This actually sends out the commands into hardware)
- */
- void updateJointControllers(void);
-
- /**
- * @brief Update all joint values by reading from the robot structure
- */
- void updateJointValues();
-
- /**
- * @brief Update the trajectory queue by removing trajectories that are done, set the current trajectory to be executed to the next trajectory on the queue.
- * If no new trajectory is available, set the desired joint positions to the current joint positions
- * @param id The id of the last trajectory
- * @param finish_status The status of the last trajectory
- */
- void updateTrajectoryQueue(int id, int finish_status);
-
- /**
- * @brief Get the next trajectory from the queue
- & @param index The index of the requested trajectory
- */
- bool getTrajectoryFromQueue(int &index);
-
- /**
- * @brief Convert a trajectory command (from the queue) and set the current desired trajectory to it
- * @param traj_msg Trajectory message received on a topic or a service
- * @param id The associated id for the trajectory command
- */
- void setTrajectoryCmdFromMsg(manipulation_msgs::JointTraj traj_msg, int id);
-
- /**
- * @brief Callback when a trajectory message is received on a topic
- */
- void TrajectoryReceivedOnTopic();
-
- /**
- * @brief Service provided to set trajectories
- * @param req The request containing the trajectory to be queued up
- * @param resp The response contains the id assigned to the trajectory
- */
- bool setJointTrajSrv(pr2_mechanism_controllers::TrajectoryStart::Request &req,
- pr2_mechanism_controllers::TrajectoryStart::Response &resp);
-
- /**
- * @brief Service provided to set trajectories
- * @param req The request contains the id of the trajectory about which you need information (Use the rosmsg tool to see the fields required for the request. e.g. rosmsg show TrajectoryQuery)
- * @param resp The response contains information about the trajectory in the following fields:
- * (a) done: 1 if trajectory is done, 0 if ongoing, 2 if queued, 3 if deleted, 4 if failed, 5 if canceled
- * (b) trajectorytime: If active, the current timestamp the trajectory is at, if done the total time taken for the trajectory, if queued 0
- * (c) jointnames: the names of the joints controlled by this controller
- * (d) jointpositions: the current joint positions
- */
- bool queryJointTrajSrv(pr2_mechanism_controllers::TrajectoryQuery::Request &req,
- pr2_mechanism_controllers::TrajectoryQuery::Response &resp);
-
- /**
- * @brief Service provided to cancel trajectories
- * @param req The request contains the id of the trajectory which needs to be canceled (Use the rosmsg tool to see the fields required for the request. e.g. rosmsg show TrajectoryCancel)
- */
- bool cancelJointTrajSrv(pr2_mechanism_controllers::TrajectoryCancel::Request &req,
- pr2_mechanism_controllers::TrajectoryCancel::Response &resp);
-
-
- /**
- * @brief Create a vector of trajectory point(TPoint) objects from a trajectory message
- * @param new_traj The trajectory message that needs to be converted
- * @param tp The resultant vector of TPoints
- */
- bool createTrajectoryPointsVectorFromMsg(const manipulation_msgs::JointTraj &new_traj, std::vector<trajectory::Trajectory::TPoint> &tp);
-
- /**
- * @brief Create a trajectory object from a trajectory message
- * @param new_traj The trajectory message that needs to be converted
- * @param return_trajectory The resultant trajectory object
- */
- bool createTrajectoryFromMsg(const manipulation_msgs::JointTraj &new_traj,trajectory::Trajectory &return_trajectory);
-
- /**
- * @brief Add a new trajectory request to the queue of trajectories that need to be sent out
- * @param new_traj The trajectory message that needs to be queued
- * @param id The id of the trajectory to be queued
- */
- void addTrajectoryToQueue(manipulation_msgs::JointTraj new_traj, int id);
-
- /**
- * @brief Preempt the current trajectory queue
- * @param new_traj The trajectory message that needs to executed
- * @param id The id of the trajectory
- */
- void preemptTrajectoryQueue(manipulation_msgs::JointTraj new_traj, int id);
-
- /**
- * @brief Delete a trajectory from the queue of trajectories
- * @param new_traj The trajectory message that needs to be deleted
- */
- void deleteTrajectoryFromQueue(int id);
-
- /**
- * @brief Publish diagnostic information
- */
- void publishDiagnostics();
-
- /**
- * @brief Initialized to 1 (0 represents a trajectory that keeps the robot at its current position)
- */
- int request_trajectory_id_;
-
- /**
- * @brief Initialized to 0 and then set to the requested trajectory id. Reverts to 0 when no trajectories are available on the queue
- */
- int current_trajectory_id_;
-
- /**
- * @brief A map from trajectory ids to trajectory status
- */
-// std::map<int,int> joint_trajectory_status_;
- std::vector<int> joint_trajectory_status_;
-
- /**
- * @brief A map from trajectory ids to trajectory times
- */
- std::vector<double> joint_trajectory_time_;
-// std::map<int,double>joint_trajectory_time_;
-
- /**
- * @brief Enumeration of trajectory status
- */
- enum JointTrajectoryStatus{
- ACTIVE,/*!< The current trajectory being executed. */
- DONE,/*!< Trajectory execution was finished. */
- QUEUED, /*!< Trajectory is queued for execution. */
- DELETED, /*!< Trajectory has been deleted BEFORE execution. */
- FAILED, /*!< Trajectory execution has failed. This happens if the joints fail to get to the last desired position in the trajectory. */
- CANCELED, /*!< Trajectory has been canceled (preempted by other trajectory) while active */
- DOES_NOT_EXIST, /*!< This trajectory does not exist yet */
- NUM_STATUS
- };
-
- /**
- * @brief The maximum amount of time that the controller waits after the expected completion time for a trajectory before declaring that a trajectory has failed
- */
- double trajectory_wait_timeout_;
-
- double last_diagnostic...
[truncated message content] |
