Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[7565] pkg/trunk
|
From: <vij...@us...> - 2008-12-04 03:30:14
|
Revision: 7565
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7565&view=rev
Author: vijaypradeep
Date: 2008-12-04 03:30:09 +0000 (Thu, 04 Dec 2008)
Log Message:
-----------
Added a ground_truth_position controller for the base
Modified Paths:
--------------
pkg/trunk/nav/fake_localization/CMakeLists.txt
pkg/trunk/nav/fake_localization/manifest.xml
Added Paths:
-----------
pkg/trunk/controllers/pr2_mechanism_controllers/scripts/control_base_position_ground_truth.py
pkg/trunk/nav/fake_localization/ground_truth_controller.cpp
pkg/trunk/nav/fake_localization/ground_truth_controller.launch
pkg/trunk/nav/fake_localization/ground_truth_controller.xml
Added: pkg/trunk/controllers/pr2_mechanism_controllers/scripts/control_base_position_ground_truth.py
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/scripts/control_base_position_ground_truth.py (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/scripts/control_base_position_ground_truth.py 2008-12-04 03:30:09 UTC (rev 7565)
@@ -0,0 +1,63 @@
+#!/usr/bin/env python
+# 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: Vijay Pradeep
+# (Derived from pointhead.py)
+
+PKG = "pr2_mechanism_controllers"
+
+import rostools; rostools.update_path(PKG)
+
+import sys
+import os
+import string
+from time import sleep
+
+import rospy
+from std_msgs.msg import PointStamped, Point
+from robot_msgs.msg import JointCmd
+
+def control_base_pose_odom_frame(x,y,w):
+ head_angles = rospy.Publisher('ground_truth_controller/set_cmd', Point)
+ rospy.init_node('base_position_commander', anonymous=True)
+ p = Point(x, y, w) ;
+ sleep(1)
+ head_angles.publish( p )
+ sleep(1)
+
+def usage():
+ return "%s [pos_x] [pos_y] [theta (rad)]"%sys.argv[0]
+
+if __name__ == "__main__":
+
+ if len(sys.argv) ==4:
+ control_base_pose_odom_frame(float(sys.argv[1]), float(sys.argv[2]), float(sys.argv[3]) )
+ else:
+ print usage()
+ sys.exit(1)
+
Property changes on: pkg/trunk/controllers/pr2_mechanism_controllers/scripts/control_base_position_ground_truth.py
___________________________________________________________________
Added: svn:executable
+ *
Modified: pkg/trunk/nav/fake_localization/CMakeLists.txt
===================================================================
--- pkg/trunk/nav/fake_localization/CMakeLists.txt 2008-12-04 03:29:42 UTC (rev 7564)
+++ pkg/trunk/nav/fake_localization/CMakeLists.txt 2008-12-04 03:30:09 UTC (rev 7565)
@@ -2,3 +2,4 @@
include(rosbuild)
rospack(fake_localization)
rospack_add_executable(fake_localization fake_localization.cpp)
+rospack_add_executable(ground_truth_controller ground_truth_controller.cpp)
Added: pkg/trunk/nav/fake_localization/ground_truth_controller.cpp
===================================================================
--- pkg/trunk/nav/fake_localization/ground_truth_controller.cpp (rev 0)
+++ pkg/trunk/nav/fake_localization/ground_truth_controller.cpp 2008-12-04 03:30:09 UTC (rev 7565)
@@ -0,0 +1,179 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+
+#include <ros/node.h>
+#include <ros/time.h>
+#include <std_msgs/PoseWithRatesStamped.h>
+#include <std_msgs/BaseVel.h>
+#include "control_toolbox/base_position_pid.h"
+#include "tf/transform_datatypes.h"
+
+using namespace std ;
+
+namespace fake_localization
+{
+
+class GroundTruthController : public ros::node
+{
+public:
+ std_msgs::PoseWithRatesStamped m_ground_truth_ ; //!< Message on which we receive ground truth info
+ control_toolbox::BasePositionPid base_position_pid_ ; //!< Does the PID math for controlling the robot
+ tf::Vector3 xyt_target_ ; //!< The ground truth pose we want to acheive
+ bool first_time_ ;
+ ros::Time prev_time_ ;
+ std_msgs::Point cmd_ ;
+
+ GroundTruthController() : ros::node("ground_truth_controller")
+ {
+ xyt_target_.setX(0) ;
+ xyt_target_.setY(0) ;
+ xyt_target_.setZ(0) ;
+ }
+
+ ~GroundTruthController() { }
+
+ bool initController()
+ {
+ printf("initController()\n") ;
+
+ string xml_config ;
+ param("~xml_config", xml_config, string("")) ;
+ subscribe("base_pose_ground_truth", m_ground_truth_, &GroundTruthController::updateControl, 1) ;
+ subscribe("~set_cmd", cmd_, &GroundTruthController::setCommandCallback, this, 10) ;
+
+ advertise<std_msgs::BaseVel>("cmd_vel", 1) ;
+
+ // Initialize the BasePositionPid util via xml
+ TiXmlDocument xml ;
+ xml.Parse(xml_config.c_str()) ;
+ TiXmlElement *config = xml.RootElement() ;
+ if (config == NULL)
+ {
+ ROS_WARN("Error opening XML file") ;
+ return false ;
+ }
+ bool result = base_position_pid_.initXml(config) ;
+ if (!result)
+ {
+ ROS_WARN("Error loading BasePositionPid xml") ;
+ return false ;
+ }
+
+ first_time_ = true ;
+
+ return true ;
+ }
+
+ /**
+ * Grabs the current ground truth message and computes the next command, and published the command
+ */
+ void updateControl()
+ {
+ //printf("Got ground truth\n") ;
+
+ if (first_time_)
+ {
+ prev_time_ = m_ground_truth_.header.stamp ;
+ first_time_ = false ;
+ return ;
+ }
+
+ ros::Time cur_time = m_ground_truth_.header.stamp ;
+ ros::Duration time_elapsed = cur_time - prev_time_ ;
+ if (time_elapsed.toSec() < .05)
+ return ;
+
+ tf::Transform ground_truth_pose ;
+
+ tf::PoseMsgToTF(m_ground_truth_.pos, ground_truth_pose) ;
+
+ //! \todo Compute yaw angle in a more stable way
+ double yaw,pitch,roll ;
+ ground_truth_pose.getBasis().getEulerZYX(yaw, pitch, roll) ;
+
+ tf::Vector3 xyt_current(ground_truth_pose.getOrigin().x(), ground_truth_pose.getOrigin().y(), yaw) ;
+
+
+ // Determine next velocity to command
+ tf::Vector3 vel_cmd ;
+ vel_cmd = base_position_pid_.updateControl(xyt_target_, xyt_current, time_elapsed.toSec()) ;
+
+ std_msgs::BaseVel base_vel ;
+ base_vel.vx = vel_cmd.x() ;
+ base_vel.vy = vel_cmd.y() ;
+ base_vel.vw = vel_cmd.z() ;
+
+ publish("cmd_vel", base_vel) ;
+
+ prev_time_ = cur_time ;
+ }
+
+ void setCommandCallback()
+ {
+ setCommand(cmd_.x, cmd_.y, cmd_.z) ;
+ }
+
+ void setCommand(double x, double y, double w)
+ {
+ ROS_INFO("BasePositionControllerNode:: Odom Frame Position Command: %f %f %f\n", x, y, w) ;
+
+ //! \todo Mutex this data type
+ xyt_target_.setX(x) ;
+ xyt_target_.setY(y) ;
+ xyt_target_.setZ(w) ;
+ }
+
+} ;
+
+}
+
+using namespace fake_localization ;
+
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv);
+
+ GroundTruthController controller ;
+ controller.initController() ;
+
+ controller.spin() ;
+
+ ros::fini() ;
+
+ return 0 ;
+
+
+
+}
Added: pkg/trunk/nav/fake_localization/ground_truth_controller.launch
===================================================================
--- pkg/trunk/nav/fake_localization/ground_truth_controller.launch (rev 0)
+++ pkg/trunk/nav/fake_localization/ground_truth_controller.launch 2008-12-04 03:30:09 UTC (rev 7565)
@@ -0,0 +1,7 @@
+
+<launch>
+
+ <node pkg="fake_localization" type="ground_truth_controller" name="ground_truth_controller" args="" output="screen">
+ <param name="xml_config" command="$(find xacro)/xacro.py '$(find fake_localization)/ground_truth_controller.xml'" />
+ </node>
+</launch>
\ No newline at end of file
Added: pkg/trunk/nav/fake_localization/ground_truth_controller.xml
===================================================================
--- pkg/trunk/nav/fake_localization/ground_truth_controller.xml (rev 0)
+++ pkg/trunk/nav/fake_localization/ground_truth_controller.xml 2008-12-04 03:30:09 UTC (rev 7565)
@@ -0,0 +1,5 @@
+<base_position_pid>
+ <pid_x p="2.0" d="0" i="0.00" iClamp="0.1" />
+ <pid_y p="2.0" d="0" i="0.00" iClamp="0.1" />
+ <pid_w p="0.5" d="0" i="0.00" iClamp="0.1" />
+</base_position_pid>
Modified: pkg/trunk/nav/fake_localization/manifest.xml
===================================================================
--- pkg/trunk/nav/fake_localization/manifest.xml 2008-12-04 03:29:42 UTC (rev 7564)
+++ pkg/trunk/nav/fake_localization/manifest.xml 2008-12-04 03:30:09 UTC (rev 7565)
@@ -8,4 +8,5 @@
<depend package="robot_msgs" />
<depend package="tf" />
<depend package="angles" />
+ <depend package="control_toolbox" />
</package>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
