Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[20185] pkg/trunk
|
From: <stu...@us...> - 2009-07-30 21:06:33
|
Revision: 20185
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20185&view=rev
Author: stuglaser
Date: 2009-07-30 21:06:22 +0000 (Thu, 30 Jul 2009)
Log Message:
-----------
Merging in new controller spawning changes.
Modified 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_kinematics.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_controller.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_position_controller.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_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_qualification.h
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/pr2_arm_dynamics_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_gripper_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/src/head_position_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/joint_trajectory_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_base_controller.cpp
pkg/trunk/controllers/qualification_controllers/dynamic_verification_controllers/include/dynamic_verification_controllers/constant_torque_controller.h
pkg/trunk/controllers/qualification_controllers/dynamic_verification_controllers/include/dynamic_verification_controllers/freq_resp_controller.h
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/backlash_controller.h
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/checkout_controller.h
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/hold_set_controller.h
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/hysteresis_controller.h
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/sine_sweep_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_hybrid_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_pose_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_tff_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_trajectory_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_twist_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_twist_controller_ik.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_wrench_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/dynamic_loader_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_autotuner.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_blind_calibration_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_chain_constraint_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_chain_sine_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_effort_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_inverse_dynamics_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_pd_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_position_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_position_smoothing_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_velocity_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/probe.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/trigger_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/scripts/effect.py
pkg/trunk/controllers/robot_mechanism_controllers/scripts/position.py
pkg/trunk/controllers/robot_mechanism_controllers/scripts/velocity.py
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_pose_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_tff_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_trajectory_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_twist_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_twist_controller_ik.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_wrench_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/dynamic_loader_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_effort_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_inverse_dynamics_controller.cpp
pkg/trunk/demos/handhold/run.launch
pkg/trunk/demos/milestone2/milestone2_actions/config/controllers.xml
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_mechanism_control.h
pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_mechanism_control.cpp
pkg/trunk/highlevel/move_arm/launch/gripper_larm.launch
pkg/trunk/highlevel/move_arm/launch/gripper_rarm.launch
pkg/trunk/pr2/life_test/arm_life_test/controller_cartesian_trajectory_left.launch
pkg/trunk/pr2/life_test/arm_life_test/controller_cartesian_trajectory_right.launch
pkg/trunk/pr2/qualification/scripts/full_arm_test_spawner.py
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_head_cart/hca.launch
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_head_cart/hcb.launch
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_head_cart/hcc.launch
pkg/trunk/robot_descriptions/pr2/pr2_experimental_controllers/r_arm_hybrid_controller.xml
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_trajectory_controller2.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/pid_position_velocity_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/plug_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/trajectory_controller.h
pkg/trunk/stacks/mechanism/mechanism_bringup/scripts/calibrate.py
pkg/trunk/stacks/mechanism/mechanism_bringup/scripts/calibrate_pr2.py
pkg/trunk/stacks/mechanism/mechanism_control/CMakeLists.txt
pkg/trunk/stacks/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
pkg/trunk/stacks/mechanism/mechanism_control/scripts/mech.py
pkg/trunk/stacks/mechanism/mechanism_control/scripts/spawner.py
pkg/trunk/stacks/mechanism/mechanism_control/src/mechanism_control/mechanism.py
pkg/trunk/stacks/mechanism/mechanism_control/src/mechanism_control.cpp
pkg/trunk/stacks/mechanism/mechanism_msgs/srv/SpawnController.srv
pkg/trunk/stacks/mechanism/mechanism_msgs/srv/SwitchController.srv
pkg/trunk/stacks/mechanism/robot_state_publisher/src/joint_state_listener.cpp
pkg/trunk/stacks/pr2/pr2_alpha/prf_ethercat_reset.launch
pkg/trunk/stacks/pr2/pr2_alpha/prg_ethercat_reset.launch
pkg/trunk/stacks/pr2/pr2_alpha/teleop_joystick.launch
pkg/trunk/stacks/pr2/pr2_alpha/teleop_joystick_new.launch
pkg/trunk/stacks/pr2/pr2_alpha/teleop_keyboard.launch
pkg/trunk/stacks/pr2/pr2_alpha/wheel_odometry_calibrate.launch
pkg/trunk/stacks/pr2/pr2_default_controllers/pr2_base_controller.yaml
pkg/trunk/stacks/pr2/pr2_default_controllers/pr2_base_controller_odom.launch
pkg/trunk/stacks/pr2/pr2_default_controllers/pr2_calibration_controllers.yaml
pkg/trunk/stacks/pr2/pr2_default_controllers/pr2_odometry.yaml
pkg/trunk/stacks/pr2/pr2_etherCAT/src/main.cpp
pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/one_way_outlets.cpp
pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/outlet_detector.cpp
pkg/trunk/vision/cv_mech_turk/mainpage.dox
Added Paths:
-----------
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/plug_controller.h
pkg/trunk/demos/pr2_gazebo/coffee_cup.launch
pkg/trunk/stacks/mechanism/mechanism_control/include/mechanism_control/controller.h
pkg/trunk/stacks/mechanism/mechanism_control/include/mechanism_control/controller_handle.h
pkg/trunk/stacks/mechanism/mechanism_control/include/mechanism_control/controller_spec.h
pkg/trunk/stacks/mechanism/mechanism_control/include/mechanism_control/scheduler.h
pkg/trunk/stacks/mechanism/mechanism_control/scripts/joints_to_diagnostics.py
pkg/trunk/stacks/mechanism/mechanism_control/src/controller_handle.cpp
pkg/trunk/stacks/mechanism/mechanism_control/src/scheduler.cpp
pkg/trunk/stacks/pr2/pr2_default_controllers/pr2_joint_effort_controllers.yaml
pkg/trunk/stacks/pr2/pr2_default_controllers/pr2_joint_position_controllers.yaml
pkg/trunk/stacks/pr2/pr2_default_controllers/pr2_joint_velocity_controllers.yaml
pkg/trunk/stacks/trex/trex_pr2/nddl/nav/
pkg/trunk/stacks/trex/trex_pr2/nddl/nav/nav.nddl
Removed Paths:
-------------
pkg/trunk/pr2/life_test/arm_life_test/controller_cartesian_pose_right.launch
pkg/trunk/pr2/life_test/arm_life_test/controller_cartesian_pose_right_forearm.launch
pkg/trunk/pr2/life_test/arm_life_test/controller_cartesian_twist_right.launch
pkg/trunk/pr2/life_test/arm_life_test/controller_cartesian_wrench_right.launch
pkg/trunk/stacks/mechanism/mechanism_model/include/mechanism_model/controller.h
pkg/trunk/stacks/mechanism/mechanism_msgs/srv/KillAndSpawnControllers.srv
pkg/trunk/stacks/pr2/pr2_default_controllers/arm_joint_effort_controllers.yaml
pkg/trunk/stacks/pr2/pr2_default_controllers/arm_joint_position_controllers.yaml
pkg/trunk/stacks/pr2/pr2_default_controllers/arm_joint_velocity_controllers.yaml
pkg/trunk/stacks/pr2/pr2_default_controllers/base_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/base_controller_experimental.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/base_odometry.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/base_trajectory_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/gazebo_head_torso_lift_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/head_pan_joint_position_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/head_pan_tilt_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/head_position_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/head_servoing_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/head_tilt_joint_position_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/l_arm_cartesian_pose_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/l_arm_cartesian_tff_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/l_arm_cartesian_trajectory_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/l_arm_cartesian_twist_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/l_arm_cartesian_twist_ik_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/l_arm_cartesian_wrench_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/l_gripper_effort_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/l_gripper_position_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/pr2_base_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/pr2_base_odometry.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/r_arm_cartesian_pose_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/r_arm_cartesian_tff_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/r_arm_cartesian_trajectory_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/r_arm_cartesian_twist_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/r_arm_cartesian_twist_ik_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/r_arm_cartesian_wrench_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/r_arm_constraint_cartesian_pose_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/r_arm_constraint_cartesian_trajectory_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/r_arm_constraint_cartesian_twist_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/r_arm_constraint_cartesian_wrench_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/r_arm_joint_position_controllers.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/r_arm_joint_velocity_controllers.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/r_gripper_effort_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/r_gripper_position_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/torso_lift_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/torso_lift_vel_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/whole_body_controller.xml
Modified: 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-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -37,7 +37,7 @@
#include <ros/node.h>
#include <boost/thread/mutex.hpp>
-#include <mechanism_model/controller.h>
+#include <mechanism_control/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>
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-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_kinematics.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -36,7 +36,7 @@
*/
#include <mechanism_model/robot.h>
-#include <mechanism_model/controller.h>
+#include <mechanism_control/controller.h>
#include <robot_msgs/PoseDot.h>
#include <robot_msgs/Point.h>
#include <control_toolbox/filters.h>
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_controller.h 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_controller.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -43,7 +43,7 @@
#include "ros/node_handle.h"
-#include "mechanism_model/controller.h"
+#include "mechanism_control/controller.h"
#include "mechanism_model/robot.h"
#include "control_toolbox/pid.h"
#include "robot_mechanism_controllers/joint_velocity_controller.h"
Modified: 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-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_controller_effort.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -41,7 +41,7 @@
#ifndef CASTER_CONTROLLER_EFFORT_H
#define CASTER_CONTROLLER_EFFORT_H
-#include "mechanism_model/controller.h"
+#include "mechanism_control/controller.h"
#include "mechanism_model/robot.h"
#include "control_toolbox/pid.h"
#include "robot_mechanism_controllers/joint_velocity_controller.h"
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_position_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_position_controller.h 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_position_controller.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -41,7 +41,7 @@
#include <ros/node.h>
#include <ros/node_handle.h>
-#include <mechanism_model/controller.h>
+#include <mechanism_control/controller.h>
#include <tf/transform_listener.h>
#include <tf/message_notifier.h>
#include <tf/transform_datatypes.h>
Modified: 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-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_servoing_controller.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -41,7 +41,7 @@
#include <boost/thread/mutex.hpp>
// Controllers
-#include <mechanism_model/controller.h>
+#include <mechanism_control/controller.h>
#include <mechanism_model/joint.h>
#include <robot_mechanism_controllers/joint_velocity_controller.h>
Modified: 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-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/joint_trajectory_controller.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -37,7 +37,7 @@
#include <ros/node.h>
#include <boost/thread/mutex.hpp>
-#include <mechanism_model/controller.h>
+#include <mechanism_control/controller.h>
#include <robot_mechanism_controllers/joint_pd_controller.h>
// Services
@@ -103,6 +103,8 @@
*/
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.
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_controller.h 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_controller.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -36,7 +36,7 @@
#include <ros/node.h>
-#include <mechanism_model/controller.h>
+#include <mechanism_control/controller.h>
#include <robot_mechanism_controllers/joint_position_smoothing_controller.h>
#include <realtime_tools/realtime_publisher.h>
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_qualification.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_qualification.h 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_qualification.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -36,7 +36,7 @@
#include <ros/node.h>
-#include <mechanism_model/controller.h>
+#include <mechanism_control/controller.h>
#include <robot_mechanism_controllers/joint_velocity_controller.h>
// Services
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-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -36,7 +36,7 @@
#include <ros/node.h>
-#include <mechanism_model/controller.h>
+#include <mechanism_control/controller.h>
#include <robot_mechanism_controllers/joint_position_smoothing_controller.h>
#include <realtime_tools/realtime_publisher.h>
@@ -70,6 +70,7 @@
~LaserScannerTrajController() ;
bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+ bool init(mechanism::RobotState *robot, const ros::NodeHandle &n);
virtual void update() ;
@@ -134,6 +135,7 @@
void update() ;
bool initXml(mechanism::RobotState *robot, TiXmlElement *config) ;
+ bool init(mechanism::RobotState *robot, const ros::NodeHandle &n);
// Message Callbacks
void setPeriodicCmd() ;
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_arm_dynamics_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_arm_dynamics_controller.h 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_arm_dynamics_controller.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -37,7 +37,7 @@
#include <ros/node.h>
#include <boost/thread/mutex.hpp>
-#include <mechanism_model/controller.h>
+#include <mechanism_control/controller.h>
#include <mechanism_model/joint.h>
#include <robot_mechanism_controllers/joint_position_controller.h>
#include <robot_mechanism_controllers/joint_velocity_controller.h>
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_gripper_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_gripper_controller.h 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_gripper_controller.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -37,7 +37,7 @@
#include <ros/node.h>
#include <mechanism_model/robot.h>
-#include <mechanism_model/controller.h>
+#include <mechanism_control/controller.h>
#include <robot_mechanism_controllers/joint_effort_controller.h>
#include <pr2_mechanism_controllers/GripperControllerCmd.h>
#include <pr2_msgs/GripperControllerState.h>
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/head_position_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/head_position_controller.cpp 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/head_position_controller.cpp 2009-07-30 21:06:22 UTC (rev 20185)
@@ -91,11 +91,6 @@
robot_state_ = robot_state;
// get a pointer to the wrench controller
- MechanismControl* mc;
- if (!MechanismControl::Instance(mc)){
- ROS_ERROR("HeadPositionController: could not get instance of mechanism control");
- return false;
- }
std::string pan_output;
if (!node_.getParam("pan_output", pan_output))
{
@@ -103,7 +98,7 @@
node_.getNamespace().c_str());
return false;
}
- if (!mc->getControllerByName<JointPositionController>(pan_output, head_pan_controller_))
+ if (!getController<JointPositionController>(pan_output, AFTER_ME, head_pan_controller_))
{
ROS_ERROR("HeadPositionController: could not connect to the pan joint controller %s (namespace: %s)",
pan_output.c_str(), node_.getNamespace().c_str());
@@ -117,7 +112,7 @@
node_.getNamespace().c_str());
return false;
}
- if (!mc->getControllerByName<JointPositionController>(tilt_output, head_tilt_controller_))
+ if (!getController<JointPositionController>(tilt_output, AFTER_ME, head_tilt_controller_))
{
ROS_ERROR("HeadPositionController: could not connect to the tilt joint controller %s (namespace: %s)",
tilt_output.c_str(), node_.getNamespace().c_str());
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/joint_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/joint_trajectory_controller.cpp 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/joint_trajectory_controller.cpp 2009-07-30 21:06:22 UTC (rev 20185)
@@ -58,22 +58,46 @@
next_free_index_ = 0;
current_trajectory_index_ = 0;
trajectory_preempted_ = false;
+ joint_trajectory_ = NULL;
}
JointTrajectoryController::~JointTrajectoryController()
{
for(unsigned int i=0; i<joint_pv_controllers_.size();++i)
- delete joint_pv_controllers_[i];
+ if (joint_pv_controllers_[i])
+ delete joint_pv_controllers_[i];
stopPublishers();
unadvertiseServices();
unsubscribeTopics();
- delete joint_trajectory_;
+ if (joint_trajectory_)
+ delete joint_trajectory_;
}
+bool JointTrajectoryController::init(mechanism::RobotState *robot, const ros::NodeHandle& n)
+{
+ // get xml from parameter server
+ string xml_string;
+ if (!n.getParam("xml", xml_string)){
+ ROS_ERROR("Could not read xml from parameter server");
+ return false;
+ }
+
+ TiXmlDocument xml_doc;
+ xml_doc.Parse(xml_string.c_str());
+ TiXmlElement *xml = xml_doc.FirstChildElement("controller");
+ if (!xml){
+ ROS_ERROR("could not parse xml %s",xml_string.c_str());
+ return false;
+ }
+
+ return initXml(robot, xml);
+}
+
+
bool JointTrajectoryController::initXml(mechanism::RobotState * robot, TiXmlElement * config)
{
name_ = config->Attribute("name");
@@ -143,11 +167,10 @@
void JointTrajectoryController::stopPublishers()
{
- controller_state_publisher_->stop();
- diagnostics_publisher_->stop();
-
- delete controller_state_publisher_;
- delete diagnostics_publisher_;
+ if (controller_state_publisher_)
+ delete controller_state_publisher_;
+ if (diagnostics_publisher_)
+ delete diagnostics_publisher_;
}
void JointTrajectoryController::advertiseServices()
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-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp 2009-07-30 21:06:22 UTC (rev 20185)
@@ -144,6 +144,26 @@
return true ;
}
+bool LaserScannerTrajController::init(mechanism::RobotState *robot, const ros::NodeHandle &n)
+{
+ std::string xml;
+ if (!n.getParam("xml", xml))
+ {
+ ROS_ERROR("No XML given (namespace: %s)", n.getNamespace().c_str());
+ return false;
+ }
+
+ TiXmlDocument doc;
+ doc.Parse(xml.c_str());
+ if (!doc.RootElement())
+ {
+ ROS_ERROR("Error parsing XML (namespace: %s)", n.getNamespace().c_str());
+ return false;
+ }
+
+ return initXml(robot, doc.RootElement());
+}
+
void LaserScannerTrajController::update()
{
if (!joint_state_->calibrated_)
@@ -494,6 +514,28 @@
return true ;
}
+bool LaserScannerTrajControllerNode::init(mechanism::RobotState *robot,
+ const ros::NodeHandle &n)
+{
+ std::string xml;
+ if (!n.getParam("xml", xml))
+ {
+ ROS_ERROR("No XML given (namespace: %s)", n.getNamespace().c_str());
+ return false;
+ }
+
+ TiXmlDocument doc;
+ doc.Parse(xml.c_str());
+ if (!doc.RootElement())
+ {
+ ROS_ERROR("Error parsing XML (namespace: %s)", n.getNamespace().c_str());
+ return false;
+ }
+
+ return initXml(robot, doc.RootElement());
+}
+
+
bool LaserScannerTrajControllerNode::setPeriodicSrv(pr2_srvs::SetPeriodicCmd::Request &req,
pr2_srvs::SetPeriodicCmd::Response &res)
{
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-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_base_controller.cpp 2009-07-30 21:06:22 UTC (rev 20185)
@@ -156,7 +156,6 @@
ROS_ERROR("Could not initialize joint velocity controller for %s",base_kin_.wheel_[j].joint_name_.c_str());
}
}
- ROS_INFO("PR2 base controller initialized");
return true;
}
Modified: pkg/trunk/controllers/qualification_controllers/dynamic_verification_controllers/include/dynamic_verification_controllers/constant_torque_controller.h
===================================================================
--- pkg/trunk/controllers/qualification_controllers/dynamic_verification_controllers/include/dynamic_verification_controllers/constant_torque_controller.h 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/qualification_controllers/dynamic_verification_controllers/include/dynamic_verification_controllers/constant_torque_controller.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -50,7 +50,7 @@
#include <dynamic_verification_controllers/DynamicResponseData.h>
#include <realtime_tools/realtime_publisher.h>
#include <realtime_tools/realtime_srv_call.h>
-#include <mechanism_model/controller.h>
+#include <mechanism_control/controller.h>
namespace controller
{
Modified: pkg/trunk/controllers/qualification_controllers/dynamic_verification_controllers/include/dynamic_verification_controllers/freq_resp_controller.h
===================================================================
--- pkg/trunk/controllers/qualification_controllers/dynamic_verification_controllers/include/dynamic_verification_controllers/freq_resp_controller.h 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/qualification_controllers/dynamic_verification_controllers/include/dynamic_verification_controllers/freq_resp_controller.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -50,7 +50,7 @@
#include <dynamic_verification_controllers/DynamicResponseData.h>
#include <realtime_tools/realtime_publisher.h>
#include <realtime_tools/realtime_srv_call.h>
-#include <mechanism_model/controller.h>
+#include <mechanism_control/controller.h>
#include <robot_mechanism_controllers/joint_velocity_controller.h>
namespace controller
Modified: pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/backlash_controller.h
===================================================================
--- pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/backlash_controller.h 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/backlash_controller.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -48,7 +48,7 @@
#include <diagnostic_msgs/DiagnosticMessage.h>
#include <realtime_tools/realtime_publisher.h>
#include <realtime_tools/realtime_srv_call.h>
-#include <mechanism_model/controller.h>
+#include <mechanism_control/controller.h>
#include <control_toolbox/sine_sweep.h>
#include <robot_mechanism_controllers/joint_velocity_controller.h>
#include <joint_qualification_controllers/TestData.h>
Modified: pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/checkout_controller.h
===================================================================
--- pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/checkout_controller.h 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/checkout_controller.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -54,7 +54,7 @@
#include <joint_qualification_controllers/RobotData.h>
#include <realtime_tools/realtime_publisher.h>
#include <realtime_tools/realtime_srv_call.h>
-#include <mechanism_model/controller.h>
+#include <mechanism_control/controller.h>
Modified: pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/hold_set_controller.h
===================================================================
--- pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/hold_set_controller.h 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/hold_set_controller.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -51,7 +51,7 @@
#include <joint_qualification_controllers/HoldSetData.h>
#include <realtime_tools/realtime_publisher.h>
#include <realtime_tools/realtime_srv_call.h>
-#include <mechanism_model/controller.h>
+#include <mechanism_control/controller.h>
#include <robot_mechanism_controllers/joint_position_controller.h>
#include <control_toolbox/dither.h>
Modified: pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/hysteresis_controller.h
===================================================================
--- pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/hysteresis_controller.h 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/hysteresis_controller.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -52,7 +52,7 @@
#include <joint_qualification_controllers/TestData.h>
#include <realtime_tools/realtime_publisher.h>
#include <realtime_tools/realtime_srv_call.h>
-#include <mechanism_model/controller.h>
+#include <mechanism_control/controller.h>
#include <robot_mechanism_controllers/joint_velocity_controller.h>
namespace controller
Modified: pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/sine_sweep_controller.h
===================================================================
--- pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/sine_sweep_controller.h 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/sine_sweep_controller.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -48,7 +48,7 @@
#include <joint_qualification_controllers/TestData.h>
#include <realtime_tools/realtime_publisher.h>
#include <realtime_tools/realtime_srv_call.h>
-#include <mechanism_model/controller.h>
+#include <mechanism_control/controller.h>
#include <control_toolbox/sine_sweep.h>
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_hybrid_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_hybrid_controller.h 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_hybrid_controller.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -32,7 +32,7 @@
#include "ros/node.h"
#include "boost/scoped_ptr.hpp"
-#include "mechanism_model/controller.h"
+#include "mechanism_control/controller.h"
#include <kdl/chain.hpp>
#include <kdl/frames.hpp>
#include "mechanism_model/chain.h"
@@ -56,6 +56,7 @@
~CartesianHybridController() {}
virtual bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+ virtual bool init(mechanism::RobotState *robot, const ros::NodeHandle &n);
virtual void update(void);
virtual bool starting();
@@ -72,6 +73,10 @@
control_toolbox::Pid pose_pids_[6]; // (x,y,z) position, then (x,y,z) rot
control_toolbox::Pid twist_pids_[6];
+ control_toolbox::PidGainsSetter pose_pid_tuner_;
+ control_toolbox::PidGainsSetter pose_rot_pid_tuner_;
+ control_toolbox::PidGainsSetter twist_pid_tuner_;
+ control_toolbox::PidGainsSetter twist_rot_pid_tuner_;
// x, y, z, rx, ry, rz
int mode_[6];
@@ -91,6 +96,8 @@
bool use_filter_;
filters::FilterChain<double> twist_filter_;
+
+ ros::NodeHandle node_;
};
class CartesianHybridControllerNode : public Controller
@@ -100,6 +107,7 @@
~CartesianHybridControllerNode();
virtual bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+ virtual bool init(mechanism::RobotState *robot, const ros::NodeHandle &n);
virtual void update(void);
virtual bool starting() { return c_.starting(); }
@@ -109,10 +117,10 @@
robot_srvs::SetPoseStamped::Response &resp);
private:
+ ros::NodeHandle node_;
std::string name_;
tf::TransformListener TF;
CartesianHybridController c_;
- ros::Node *node_;
//manipulation_msgs::TaskFrameFormalism command_msg_;
boost::scoped_ptr<tf::MessageNotifier<manipulation_msgs::TaskFrameFormalism> > command_notifier_;
@@ -121,10 +129,6 @@
unsigned int loop_count_;
boost::scoped_ptr<realtime_tools::RealtimePublisher<robot_mechanism_controllers::CartesianHybridState> > pub_state_;
boost::scoped_ptr<realtime_tools::RealtimePublisher<tf::tfMessage> > pub_tf_;
- control_toolbox::PidGainsSetter pose_pid_tuner_;
- control_toolbox::PidGainsSetter pose_rot_pid_tuner_;
- control_toolbox::PidGainsSetter twist_pid_tuner_;
- control_toolbox::PidGainsSetter twist_rot_pid_tuner_;
};
}
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_pose_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_pose_controller.h 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_pose_controller.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -42,7 +42,7 @@
#include <ros/node.h>
#include <robot_msgs/PoseStamped.h>
#include <robot_msgs/Twist.h>
-#include <mechanism_model/controller.h>
+#include <mechanism_control/controller.h>
#include <realtime_tools/realtime_publisher.h>
#include <boost/scoped_ptr.hpp>
#include "robot_mechanism_controllers/cartesian_twist_controller.h"
@@ -56,9 +56,7 @@
CartesianPoseController();
~CartesianPoseController();
- bool initXml(mechanism::RobotState *robot_state, TiXmlElement *config);
bool init(mechanism::RobotState *robot, const ros::NodeHandle &n);
-
bool starting();
void update();
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_tff_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_tff_controller.h 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_tff_controller.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -41,7 +41,7 @@
#include <ros/node.h>
#include <manipulation_msgs/TaskFrameFormalism.h>
#include <robot_msgs/Twist.h>
-#include <mechanism_model/controller.h>
+#include <mechanism_control/controller.h>
#include <tf/transform_datatypes.h>
#include <control_toolbox/pid.h>
#include <boost/scoped_ptr.hpp>
@@ -56,16 +56,15 @@
CartesianTFFController();
~CartesianTFFController();
- bool initXml(mechanism::RobotState *robot_state, TiXmlElement *config);
-
+ bool init(mechanism::RobotState *robot_state, const ros::NodeHandle& n);
bool starting();
void update();
- void command();
+ void command(const manipulation_msgs::TaskFrameFormalismConstPtr& tff_msg);
private:
- ros::Node* node_;
- std::string controller_name_;
+ ros::NodeHandle node_;
+ ros::Subscriber sub_command_;
double last_time_;
// pid controllers
@@ -90,7 +89,6 @@
KDL::Twist position_, twist_meas_;
KDL::Frame pose_meas_, pose_meas_old_;
- manipulation_msgs::TaskFrameFormalism tff_msg_;
boost::scoped_ptr<realtime_tools::RealtimePublisher<robot_msgs::Twist> > state_position_publisher_;
unsigned int loop_count_;
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_trajectory_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_trajectory_controller.h 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_trajectory_controller.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -43,7 +43,7 @@
#include <tf/message_notifier.h>
#include <ros/node.h>
#include <robot_msgs/PoseStamped.h>
-#include <mechanism_model/controller.h>
+#include <mechanism_control/controller.h>
#include <robot_mechanism_controllers/cartesian_pose_controller.h>
#include <deprecated_srvs/MoveToPose.h>
#include <std_srvs/Empty.h>
@@ -58,11 +58,12 @@
CartesianTrajectoryController();
~CartesianTrajectoryController();
- bool initXml(mechanism::RobotState *robot_state, TiXmlElement *config);
+ bool init(mechanism::RobotState *robot_state, const ros::NodeHandle& n);
bool starting();
void update();
- bool moveTo(const robot_msgs::PoseStamped& pose, const robot_msgs::Twist& tolerance=robot_msgs::Twist(), double duration=0);
+ bool moveTo(const robot_msgs::PoseStamped& pose,
+ const robot_msgs::Twist& tolerance=robot_msgs::Twist(), double duration=0);
private:
@@ -76,7 +77,9 @@
bool moveTo(deprecated_srvs::MoveToPose::Request &req, deprecated_srvs::MoveToPose::Response &resp);
bool preempt(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp);
- ros::Node* node_;
+ ros::NodeHandle node_;
+ ros::ServiceServer move_to_srv_, preempt_srv_;
+
std::string controller_name_;
double last_time_, time_started_, time_passed_, max_duration_;
bool is_moving_, request_preempt_, exceed_tolerance_;
@@ -100,7 +103,7 @@
CartesianPoseController* pose_controller_;
tf::TransformListener tf_;
- tf::MessageNotifier<robot_msgs::PoseStamped>* command_notifier_;
+ boost::scoped_ptr<tf::MessageNotifier<robot_msgs::PoseStamped> > command_notifier_;
std::string root_name_;
};
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_twist_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_twist_controller.h 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_twist_controller.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -40,7 +40,7 @@
#include <kdl/frames.hpp>
#include <ros/node.h>
#include <robot_msgs/Twist.h>
-#include <mechanism_model/controller.h>
+#include <mechanism_control/controller.h>
#include <tf/transform_datatypes.h>
#include <robot_mechanism_controllers/cartesian_wrench_controller.h>
#include <control_toolbox/pid.h>
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_twist_controller_ik.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_twist_controller_ik.h 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_twist_controller_ik.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -43,7 +43,7 @@
#include <kdl/chainiksolver.hpp>
#include <ros/node.h>
#include <robot_msgs/Twist.h>
-#include <mechanism_model/controller.h>
+#include <mechanism_control/controller.h>
#include <mechanism_model/chain.h>
#include <tf/transform_datatypes.h>
#include <control_toolbox/pid.h>
@@ -58,12 +58,11 @@
CartesianTwistControllerIk();
~CartesianTwistControllerIk();
- bool initXml(mechanism::RobotState *robot_state, TiXmlElement *config);
-
+ bool init(mechanism::RobotState *robot_state, const ros::NodeHandle& n);
bool starting();
void update();
- void command();
+ void command(const robot_msgs::TwistConstPtr& twist_msg);
// input of the controller
KDL::Twist twist_desi_;
@@ -71,8 +70,9 @@
private:
KDL::Twist twist_meas_,error,twist_out_;
- ros::Node* node_;
- std::string controller_name_;
+ ros::NodeHandle node_;
+ ros::Subscriber sub_command_;
+
double last_time_,ff_trans_,ff_rot_;
// pid controllers
@@ -89,8 +89,6 @@
KDL::JntArrayVel jnt_posvel_;
JointInverseDynamicsController* id_controller_;
-
- robot_msgs::Twist twist_msg_;
};
} // namespace
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_wrench_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_wrench_controller.h 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_wrench_controller.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -40,7 +40,7 @@
#include <kdl/chainjnttojacsolver.hpp>
#include <ros/node_handle.h>
#include <robot_msgs/Wrench.h>
-#include <mechanism_model/controller.h>
+#include <mechanism_control/controller.h>
#include <mechanism_model/chain.h>
#include <tf/transform_datatypes.h>
#include <diagnostic_msgs/DiagnosticMessage.h>
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/dynamic_loader_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/dynamic_loader_controller.h 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/dynamic_loader_controller.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -34,7 +34,7 @@
#ifndef DYNAMIC_LOADER_CONTROLLER_H
#define DYNAMIC_LOADER_CONTROLLER_H
-#include "mechanism_model/controller.h"
+#include "mechanism_control/controller.h"
#include <ltdl.h>
namespace controller {
@@ -103,7 +103,7 @@
std::vector<std::string> names_;
lt_dlhandle handle_;
- void loadLibrary(std::string& xml);
+ void loadLibrary(std::string& name);
static void unloadLibrary(std::vector<std::string> names, lt_dlhandle handle);
};
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -43,7 +43,7 @@
#include "ros/node.h"
#include "robot_msgs/Wrench.h"
#include "misc_utils/subscription_guard.h"
-#include "mechanism_model/controller.h"
+#include "mechanism_control/controller.h"
#include "tf/transform_datatypes.h"
#include "misc_utils/advertised_service_guard.h"
#include "joy/Joy.h"
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_autotuner.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_autotuner.h 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_autotuner.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -46,7 +46,7 @@
#include <ros/node.h>
#include <vector>
-#include <mechanism_model/controller.h>
+#include <mechanism_control/controller.h>
#include <control_toolbox/pid.h>
// Services
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_blind_calibration_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_blind_calibration_controller.h 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_blind_calibration_controller.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -56,7 +56,7 @@
/***************************************************/
-#include "mechanism_model/controller.h"
+#include "mechanism_control/controller.h"
#include "robot_mechanism_controllers/joint_velocity_controller.h"
#include "misc_utils/advertised_service_guard.h"
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_chain_constraint_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_chain_constraint_controller.h 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_chain_constraint_controller.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -62,7 +62,7 @@
#include "robot_msgs/Wrench.h"
#include "robot_mechanism_controllers/ChangeConstraints.h"
#include "robot_mechanism_controllers/JointConstraint.h"
-#include "mechanism_model/controller.h"
+#include "mechanism_control/controller.h"
#include "mechanism_model/chain.h"
#include "tf/transform_datatypes.h"
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_chain_sine_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_chain_sine_controller.h 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_chain_sine_controller.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -58,7 +58,7 @@
#ifndef JOINT_CHAIN_SINE_CONTROLLER_H_
#define JOINT_CHAIN_SINE_CONTROLLER_H_
-#include <mechanism_model/controller.h>
+#include <mechanism_control/controller.h>
#include <kdl/chain.hpp>
#include <kdl/chainfksolver.hpp>
#include <mechanism_model/chain.h>
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_effort_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_effort_controller.h 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_effort_controller.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -52,7 +52,7 @@
/***************************************************/
#include <ros/node.h>
-#include <mechanism_model/controller.h>
+#include <mechanism_control/controller.h>
#include "misc_utils/advertised_service_guard.h"
#include "misc_utils/subscription_guard.h"
@@ -73,6 +73,7 @@
bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
bool init(mechanism::RobotState *robot, const std::string &joint_name);
+ virtual bool init(mechanism::RobotState *robot, const ros::NodeHandle &n);
virtual bool starting() { command_ = 0.0; return true; }
virtual void update();
@@ -83,6 +84,11 @@
private:
mechanism::RobotState *robot_;
+
+ ros::NodeHandle node_;
+ ros::Subscriber sub_command_;
+ void commandCB(const std_msgs::Float64ConstPtr& msg);
+
};
/***************************************************/
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_inverse_dynamics_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_inverse_dynamics_controller.h 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_inverse_dynamics_controller.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -46,7 +46,7 @@
#include <ros/node.h>
#include <std_msgs/Float64MultiArray.h>
-#include <mechanism_model/controller.h>
+#include <mechanism_control/controller.h>
#include <mechanism_model/chain.h>
#include <tf/transform_datatypes.h>
#include <diagnostic_msgs/DiagnosticMessage.h>
@@ -64,24 +64,24 @@
JointInverseDynamicsController();
~JointInverseDynamicsController();
- bool initXml(mechanism::RobotState *robot_state, TiXmlElement *config);
-
+ bool init(mechanism::RobotState *robot_state, const ros::NodeHandle& n);
bool starting();
void update();
- void command();
-
// input of the controller
KDL::JntArrayAcc jnt_posvelacc_desi_;
KDL::Wrenches sgmnt_forces_;
private:
+ ros::NodeHandle node_;
+
int counter;
bool publishDiagnostics(int level, const std::string& message);
double last_time_;
- ros::Node* node_;
- std::string controller_name_;
+
+ ros::Publisher pub_pos_desi_, pub_pos_meas_, pub_vel_desi_, pub_vel_meas_;
+ ros::Publisher pub_acc_desi_, pub_acc_control_, pub_eff_calculated_, pub_eff_sent_;
mechanism::RobotState *robot_state_;
mechanism::Chain chain_;
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_pd_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_pd_controller.h 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_pd_controller.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -53,7 +53,7 @@
#include <ros/node.h>
-#include <mechanism_model/controller.h>
+#include <mechanism_control/controller.h>
#include <control_toolbox/pid.h>
#include "misc_utils/advertised_service_guard.h"
#include "misc_utils/subscription_guard.h"
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_position_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_position_controller.h 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_position_controller.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -54,7 +54,7 @@
#include <ros/node.h>
-#include <mechanism_model/controller.h>
+#include <mechanism_control/controller.h>
#include <control_toolbox/pid.h>
#include "misc_utils/advertised_service_guard.h"
#include "misc_utils/subscription_guard.h"
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_position_smoothing_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_position_smoothing_controller.h 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_position_smoothing_controller.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -53,7 +53,7 @@
#include <ros/node.h>
-#include <mechanism_model/controller.h>
+#include <mechanism_control/controller.h>
#include <control_toolbox/pid.h>
#include "misc_utils/advertised_service_guard.h"
#include "misc_utils/subscription_guard.h"
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_velocity_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_velocity_controller.h 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_velocity_controller.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -63,7 +63,7 @@
#include <ros/node.h>
#include <ros/node_handle.h>
-#include "mechanism_model/controller.h"
+#include "mechanism_control/controller.h"
#include "control_toolbox/pid.h"
#include "control_toolbox/pid_gains_setter.h"
#include "misc_utils/advertised_service_guard.h"
@@ -114,6 +114,7 @@
* \brief Issues commands to the joint. Should be called at regular intervals
*/
+ virtual bool starting() { pid_controller_.reset(); return true; }
virtual void update();
void getGains(double &p, double &i, double &d, double &i_max, double &i_min);
Copied: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/plug_controller.h (from rev 20184, pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/plug_controller.h)
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/plug_controller.h (rev 0)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/plug_controller.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -0,0 +1,182 @@
+/*
+ * 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 c...
[truncated message content] |
