|
From: <rob...@us...> - 2008-10-24 22:15:40
|
Revision: 5757
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5757&view=rev
Author: rob_wheeler
Date: 2008-10-24 22:15:18 +0000 (Fri, 24 Oct 2008)
Log Message:
-----------
Move SetJointCmd, GetJointCmd, and JointCmd to robot_srvs and robot_msgs
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_dynamics_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml
pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_dynamics_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_dynamics_controller.cpp
pkg/trunk/nav/teleop_base/manifest.xml
pkg/trunk/nav/teleop_base/teleop_head.cpp
Added Paths:
-----------
pkg/trunk/robot_msgs/msg/JointCmd.msg
pkg/trunk/robot_srvs/srv/GetJointCmd.srv
pkg/trunk/robot_srvs/srv/SetJointCmd.srv
Removed Paths:
-------------
pkg/trunk/controllers/pr2_mechanism_controllers/msg/JointCmd.msg
pkg/trunk/controllers/pr2_mechanism_controllers/srv/GetJointCmd.srv
pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetJointCmd.srv
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_dynamics_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_dynamics_controller.h 2008-10-24 22:05:43 UTC (rev 5756)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_dynamics_controller.h 2008-10-24 22:15:18 UTC (rev 5757)
@@ -44,8 +44,8 @@
#include <robot_mechanism_controllers/joint_effort_controller.h>
// Services
-#include <pr2_mechanism_controllers/SetJointCmd.h>
-#include <pr2_mechanism_controllers/GetJointCmd.h>
+#include <robot_srvs/SetJointCmd.h>
+#include <robot_srvs/GetJointCmd.h>
#include <pr2_mechanism_controllers/SetJointGains.h>
#include <pr2_mechanism_controllers/GetJointGains.h>
@@ -99,9 +99,9 @@
// * @brief Overloaded method for convenience
// * @param req
// */
-// void setJointCmd(pr2_mechanism_controllers::SetJointPosCmd::request &req);
+// void setJointCmd(robot_msgs::SetJointPosCmd::request &req);
- void getJointCmd(pr2_mechanism_controllers::JointCmd & cmd) const;
+ void getJointCmd(robot_msgs::JointCmd & cmd) const;
/*!
* \brief Get latest position command to the joint: revolute (angle) and prismatic (position).
@@ -216,8 +216,8 @@
* @param resp The response is empty
* @return
*/
- bool setJointSrv(pr2_mechanism_controllers::SetJointCmd::request &req,
- pr2_mechanism_controllers::SetJointCmd::response &resp);
+ bool setJointSrv(robot_srvs::SetJointCmd::request &req,
+ robot_srvs::SetJointCmd::response &resp);
/** @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
@@ -225,11 +225,11 @@
* @param resp the response, contains a JointPosCmd message with the goal of the controller
* @return
*/
- bool getJointCmd(pr2_mechanism_controllers::GetJointCmd::request &req,
- pr2_mechanism_controllers::GetJointCmd::response &resp);
+ bool getJointCmd(robot_srvs::GetJointCmd::request &req,
+ robot_srvs::GetJointCmd::response &resp);
private:
- pr2_mechanism_controllers::JointCmd msg_; //The message used by the ROS callback
+ robot_msgs::JointCmd msg_; //The message used by the ROS callback
ArmDynamicsController *c_;
};
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h 2008-10-24 22:05:43 UTC (rev 5756)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h 2008-10-24 22:15:18 UTC (rev 5757)
@@ -46,8 +46,8 @@
#include <robot_mechanism_controllers/joint_position_controller.h>
// Services
-#include <pr2_mechanism_controllers/SetJointCmd.h>
-#include <pr2_mechanism_controllers/GetJointCmd.h>
+#include <robot_srvs/SetJointCmd.h>
+#include <robot_srvs/GetJointCmd.h>
#include <std_msgs/PointStamped.h>
// Math utils
@@ -119,7 +119,7 @@
*
* \param cmd names and positions.
*/
- void getJointCmd(pr2_mechanism_controllers::JointCmd & cmd) const;
+ void getJointCmd(robot_msgs::JointCmd & cmd) const;
/*!
* \brief Returns the position of the controller in the vector.
@@ -185,8 +185,8 @@
* \param req (names, positions)
* \param resp (names, positions)
*/
- bool setJointCmd(pr2_mechanism_controllers::SetJointCmd::request &req,
- pr2_mechanism_controllers::SetJointCmd::response &resp);
+ bool setJointCmd(robot_srvs::SetJointCmd::request &req,
+ robot_srvs::SetJointCmd::response &resp);
/*!
* \brief Gets the commands for all the joints managed by the controller at once.
@@ -194,8 +194,8 @@
* \param req
* \param resp (positions)
*/
- bool getJointCmd(pr2_mechanism_controllers::GetJointCmd::request &req,
- pr2_mechanism_controllers::GetJointCmd::response &resp);
+ bool getJointCmd(robot_srvs::GetJointCmd::request &req,
+ robot_srvs::GetJointCmd::response &resp);
/*!
* \brief Tracks a point in a specified frame.
*
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml 2008-10-24 22:05:43 UTC (rev 5756)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml 2008-10-24 22:15:18 UTC (rev 5757)
@@ -16,6 +16,7 @@
<depend package="std_msgs" />
<depend package="math_utils" />
<depend package="robot_kinematics" />
+ <depend package="xenomai" />
<url>http://pr.willowgarage.com</url>
<repository>http://pr.willowgarage.com/repos</repository>
<export>
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/msg/JointCmd.msg
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/msg/JointCmd.msg 2008-10-24 22:05:43 UTC (rev 5756)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/msg/JointCmd.msg 2008-10-24 22:15:18 UTC (rev 5757)
@@ -1,4 +0,0 @@
-string[] names
-float64[] positions
-float64[] velocity
-float64[] acc
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_dynamics_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_dynamics_controller.cpp 2008-10-24 22:05:43 UTC (rev 5756)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_dynamics_controller.cpp 2008-10-24 22:15:18 UTC (rev 5757)
@@ -126,7 +126,7 @@
}
-void ArmDynamicsController::getJointCmd(pr2_mechanism_controllers::JointCmd & cmd) const
+void ArmDynamicsController::getJointCmd(robot_msgs::JointCmd & cmd) const
{
const unsigned int n = joint_effort_controllers_.size();
cmd.set_names_size(n);
@@ -317,8 +317,8 @@
return false;
}
-bool ArmDynamicsControllerNode::setJointSrv(pr2_mechanism_controllers::SetJointCmd::request &req,
- pr2_mechanism_controllers::SetJointCmd::response &resp)
+bool ArmDynamicsControllerNode::setJointSrv(robot_srvs::SetJointCmd::request &req,
+ robot_srvs::SetJointCmd::response &resp)
{
std::vector<double> pos;
std::vector<double> vel;
@@ -334,10 +334,10 @@
return true;
}
-bool ArmDynamicsControllerNode::getJointCmd(pr2_mechanism_controllers::GetJointCmd::request &req,
- pr2_mechanism_controllers::GetJointCmd::response &resp)
+bool ArmDynamicsControllerNode::getJointCmd(robot_srvs::GetJointCmd::request &req,
+ robot_srvs::GetJointCmd::response &resp)
{
- pr2_mechanism_controllers::JointCmd cmd;
+ robot_msgs::JointCmd cmd;
c_->getJointCmd(cmd);
resp.command = cmd;
return true;
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp 2008-10-24 22:05:43 UTC (rev 5756)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp 2008-10-24 22:15:18 UTC (rev 5757)
@@ -97,7 +97,7 @@
}
-void HeadPanTiltController::getJointCmd(pr2_mechanism_controllers::JointCmd & cmd) const
+void HeadPanTiltController::getJointCmd(robot_msgs::JointCmd & cmd) const
{
const unsigned int n = joint_position_controllers_.size();
cmd.set_names_size(n);
@@ -201,12 +201,12 @@
return false;
}
-bool HeadPanTiltControllerNode::setJointCmd(pr2_mechanism_controllers::SetJointCmd::request &req,
- pr2_mechanism_controllers::SetJointCmd::response &resp)
+bool HeadPanTiltControllerNode::setJointCmd(robot_srvs::SetJointCmd::request &req,
+ robot_srvs::SetJointCmd::response &resp)
{
std::vector<double> pos;
std::vector<std::string> names;
- pr2_mechanism_controllers::JointCmd cmds;
+ robot_msgs::JointCmd cmds;
req.get_positions_vec(pos);
req.get_names_vec(names);
@@ -218,10 +218,10 @@
return true;
}
-bool HeadPanTiltControllerNode::getJointCmd(pr2_mechanism_controllers::GetJointCmd::request &req,
- pr2_mechanism_controllers::GetJointCmd::response &resp)
+bool HeadPanTiltControllerNode::getJointCmd(robot_srvs::GetJointCmd::request &req,
+ robot_srvs::GetJointCmd::response &resp)
{
- pr2_mechanism_controllers::JointCmd cmd;
+ robot_msgs::JointCmd cmd;
c_->getJointCmd(cmd);
resp.command = cmd;
return true;
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/srv/GetJointCmd.srv
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/srv/GetJointCmd.srv 2008-10-24 22:05:43 UTC (rev 5756)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/srv/GetJointCmd.srv 2008-10-24 22:15:18 UTC (rev 5757)
@@ -1,2 +0,0 @@
----
-JointCmd command
\ No newline at end of file
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetJointCmd.srv
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetJointCmd.srv 2008-10-24 22:05:43 UTC (rev 5756)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetJointCmd.srv 2008-10-24 22:15:18 UTC (rev 5757)
@@ -1,9 +0,0 @@
-float64[] positions
-float64[] velocity
-float64[] acc
-string[] names
----
-float64[] positions
-float64[] velocity
-float64[] acc
-string[] names
\ No newline at end of file
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_dynamics_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_dynamics_controller.cpp 2008-10-24 22:05:43 UTC (rev 5756)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_dynamics_controller.cpp 2008-10-24 22:15:18 UTC (rev 5757)
@@ -34,7 +34,7 @@
#include <libTF/libTF.h>
#include <ros/node.h>
-#include <pr2_mechanism_controllers/SetJointCmd.h>
+#include <robot_srvs/SetJointCmd.h>
#include <std_msgs/TransformWithRateStamped.h>
#include <std_msgs/BaseVel.h>
#include <std_msgs/RobotBase2DOdom.h>
@@ -75,8 +75,8 @@
signal(SIGTERM, finalize);
/*********** Start moving the arm ************/
- pr2_mechanism_controllers::SetJointCmd::request req;
- pr2_mechanism_controllers::SetJointCmd::response resp;
+ robot_srvs::SetJointCmd::request req;
+ robot_srvs::SetJointCmd::response resp;
int num_joints = 7;
Modified: pkg/trunk/nav/teleop_base/manifest.xml
===================================================================
--- pkg/trunk/nav/teleop_base/manifest.xml 2008-10-24 22:05:43 UTC (rev 5756)
+++ pkg/trunk/nav/teleop_base/manifest.xml 2008-10-24 22:15:18 UTC (rev 5757)
@@ -3,5 +3,5 @@
<license>BSD</license>
<depend package="joy"/>
<depend package="std_msgs"/>
- <depend package="pr2_mechanism_controllers"/>
+ <depend package="robot_srvs"/>
</package>
Modified: pkg/trunk/nav/teleop_base/teleop_head.cpp
===================================================================
--- pkg/trunk/nav/teleop_base/teleop_head.cpp 2008-10-24 22:05:43 UTC (rev 5756)
+++ pkg/trunk/nav/teleop_base/teleop_head.cpp 2008-10-24 22:15:18 UTC (rev 5757)
@@ -3,7 +3,7 @@
#include "ros/node.h"
#include "joy/Joy.h"
-#include "pr2_mechanism_controllers/SetJointCmd.h"
+#include "robot_srvs/SetJointCmd.h"
using namespace ros;
@@ -71,8 +71,8 @@
req_tilt = std::max(std::min(req_tilt, max_tilt), -max_tilt);
}
- pr2_mechanism_controllers::SetJointCmd::request req;
- pr2_mechanism_controllers::SetJointCmd::response res;
+ robot_srvs::SetJointCmd::request req;
+ robot_srvs::SetJointCmd::response res;
req.positions.push_back(req_pan);
req.positions.push_back(req_tilt);
req.velocity.push_back(0.0);
Copied: pkg/trunk/robot_msgs/msg/JointCmd.msg (from rev 5691, pkg/trunk/controllers/pr2_mechanism_controllers/msg/JointCmd.msg)
===================================================================
--- pkg/trunk/robot_msgs/msg/JointCmd.msg (rev 0)
+++ pkg/trunk/robot_msgs/msg/JointCmd.msg 2008-10-24 22:15:18 UTC (rev 5757)
@@ -0,0 +1,4 @@
+string[] names
+float64[] positions
+float64[] velocity
+float64[] acc
Copied: pkg/trunk/robot_srvs/srv/GetJointCmd.srv (from rev 5691, pkg/trunk/controllers/pr2_mechanism_controllers/srv/GetJointCmd.srv)
===================================================================
--- pkg/trunk/robot_srvs/srv/GetJointCmd.srv (rev 0)
+++ pkg/trunk/robot_srvs/srv/GetJointCmd.srv 2008-10-24 22:15:18 UTC (rev 5757)
@@ -0,0 +1,2 @@
+---
+robot_msgs/JointCmd command
Copied: pkg/trunk/robot_srvs/srv/SetJointCmd.srv (from rev 5691, pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetJointCmd.srv)
===================================================================
--- pkg/trunk/robot_srvs/srv/SetJointCmd.srv (rev 0)
+++ pkg/trunk/robot_srvs/srv/SetJointCmd.srv 2008-10-24 22:15:18 UTC (rev 5757)
@@ -0,0 +1,9 @@
+float64[] positions
+float64[] velocity
+float64[] acc
+string[] names
+---
+float64[] positions
+float64[] velocity
+float64[] acc
+string[] names
\ No newline at end of file
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|