Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[8180] pkg/trunk
|
From: <vij...@us...> - 2008-12-16 18:44:38
|
Revision: 8180
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8180&view=rev
Author: vijaypradeep
Date: 2008-12-16 18:44:27 +0000 (Tue, 16 Dec 2008)
Log Message:
-----------
LaserScannerTrajController works in gazebo. Added PeriodicCmd message type and script to interface with controller.
Still need to test controller on hardware. Also moved xml for this controller to pr2_experimental_controllers.
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp
Added Paths:
-----------
pkg/trunk/controllers/pr2_mechanism_controllers/msg/PeriodicCmd.msg
pkg/trunk/controllers/pr2_mechanism_controllers/scripts/send_periodic_cmd.py
pkg/trunk/robot_descriptions/pr2/pr2_experimental_controllers/laser_tilt/
pkg/trunk/robot_descriptions/pr2/pr2_experimental_controllers/laser_tilt/laser_tilt_controller.xml
Removed Paths:
-------------
pkg/trunk/robot_descriptions/wg_robot_description/tilt_laser_deprecated/tilt_laser_traj_controller.xml
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 2008-12-16 18:36:11 UTC (rev 8179)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h 2008-12-16 18:44:27 UTC (rev 8180)
@@ -43,6 +43,7 @@
// Messages
#include <pr2_mechanism_controllers/LaserScannerSignal.h>
+#include <pr2_mechanism_controllers/PeriodicCmd.h>
// Services
#include <robot_mechanism_controllers/SetCommand.h>
@@ -60,31 +61,35 @@
public:
LaserScannerTrajController() ;
~LaserScannerTrajController() ;
-
-
+
bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
- /*!
- * \brief Issues commands to the joint. Should be called at regular intervals
- */
- virtual void update();
+ virtual void update() ;
- void setTrajectory(const std::vector<trajectory::Trajectory::TPoint>& traj_points, double max_rate, double max_acc, std::string interp ) ;
- void sampleTrajectory(double time_from_start) ;
+ void setPeriodicCmd(const pr2_mechanism_controllers::PeriodicCmd& cmd) ;
+ void setTrajectory(const std::vector<trajectory::Trajectory::TPoint>& traj_points,
+ double max_rate, double max_acc, std::string interp ) ;
+
+ //! \brief Returns what time we're currently at in the profile being executed
+ inline double getCurProfileTime() ;
+ //! \brief Returns the length (in seconds) of our current profile
+ inline double getProfileDuration() ;
+
private:
mechanism::RobotState *robot_ ;
mechanism::JointState *joint_state_ ; // Need this to check the calibrated flag on the joint
-
+
ros::thread::mutex traj_lock_ ; // Mutex for traj_
trajectory::Trajectory traj_ ; // Stores the current trajectory being executed
JointPositionSmoothController joint_position_controller_ ; // The PID position controller that is doing all the under-the-hood controls stuff
-
- double traj_start_time_ ;
-
- unsigned int update_count_ ;
-
+
+ double traj_start_time_ ; // The time that the trajectory was started (in seconds)
+ double traj_duration_ ; // The length of the current profile (in seconds)
+
+ double max_rate_ ; // Max allowable rate/velocity
+ double max_acc_ ; // Max allowable acceleration
};
class LaserScannerTrajControllerNode : public Controller
@@ -98,15 +103,20 @@
bool initXml(mechanism::RobotState *robot, TiXmlElement *config) ;
// Message Callbacks
- void setCommand() ;
+ void setPeriodicCmd() ;
private:
- LaserScannerTrajController *c_ ;
+ ros::node *node_ ;
+ LaserScannerTrajController c_ ;
std::string service_prefix_ ;
- ros::node *node_ ;
+ double prev_profile_time_ ; //!< The time in the current profile when update() was last called
- std_msgs::Float64 cmd_; //! \todo Change interface to accept arbitrary trajectories
+ pr2_mechanism_controllers::PeriodicCmd cmd_ ;
+
+ pr2_mechanism_controllers::LaserScannerSignal m_scanner_signal_ ; //!< Stores the message that we want to send at the end of each sweep, and halfway through each sweep
+ bool need_to_send_msg_ ; //!< Tracks whether we still need to send out the m_scanner_signal_ message.
+ misc_utils::RealtimePublisher <pr2_mechanism_controllers::LaserScannerSignal>* publisher_ ; //!< Publishes the m_scanner_signal msg from the update() realtime loop
};
}
Added: pkg/trunk/controllers/pr2_mechanism_controllers/msg/PeriodicCmd.msg
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/msg/PeriodicCmd.msg (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/msg/PeriodicCmd.msg 2008-12-16 18:44:27 UTC (rev 8180)
@@ -0,0 +1,5 @@
+Header header
+string profile
+float64 period
+float64 amplitude
+float64 offset
Added: pkg/trunk/controllers/pr2_mechanism_controllers/scripts/send_periodic_cmd.py
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/scripts/send_periodic_cmd.py (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/scripts/send_periodic_cmd.py 2008-12-16 18:44:27 UTC (rev 8180)
@@ -0,0 +1,52 @@
+#!/usr/bin/env python
+
+PKG = "pr2_mechanism_controllers"
+
+import rostools; rostools.update_path(PKG)
+
+import sys
+import os
+import string
+
+import rospy
+from std_msgs import *
+
+from pr2_mechanism_controllers.msg import PeriodicCmd
+from time import sleep
+
+def print_usage(exit_code = 0):
+ print '''Usage:
+ send_periodic_cmd.py [controller] [profile] [period] [amplitude] [offset]
+ - [profile] - Possible options are linear, linear_blended, sine
+ - [period] - Time for one entire cycle to execute (in seconds)
+ - [amplitude] - Distance max value to min value of profile (In radians for laser_tilt controller)
+ - [offset] - Constant cmd to add to profile (offset=0 results in profile centered around 0)
+'''
+ sys.exit(exit_code)
+
+if __name__ == '__main__':
+ if len(sys.argv) != 6:
+ print_usage()
+
+ cmd = PeriodicCmd()
+ controller = sys.argv[1]
+ cmd.header = rostools.msg.Header(None, None, None)
+ cmd.profile = sys.argv[2]
+ cmd.period = float (sys.argv[3])
+ cmd.amplitude = float (sys.argv[4])
+ cmd.offset = float (sys.argv[5])
+
+ print 'Sending Command to %s: ' % controller
+ print ' Profile Type: %s' % cmd.profile
+ print ' Period: %f Seconds' % cmd.period
+ print ' Amplitude: %f Radians' % cmd.amplitude
+ print ' Offset: %f Radians' % cmd.offset
+
+ command_publisher = rospy.Publisher(controller + '/set_periodic_cmd', PeriodicCmd)
+ rospy.init_node('periodic_cmd_commander', anonymous=True)
+ sleep(1)
+ command_publisher.publish( cmd )
+
+ sleep(1)
+
+ print 'Command sent!'
Property changes on: pkg/trunk/controllers/pr2_mechanism_controllers/scripts/send_periodic_cmd.py
___________________________________________________________________
Added: svn:executable
+ *
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 2008-12-16 18:36:11 UTC (rev 8179)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp 2008-12-16 18:44:27 UTC (rev 8180)
@@ -66,12 +66,25 @@
const char *jn = j->Attribute("name") ;
if (!jn)
- return false ;
+ return false ;
std::string joint_name = jn ;
joint_state_ = robot_->getJointState(joint_name) ; // Need joint state to check calibrated flag
-
+
joint_position_controller_.initXml(robot, config) ; //Pass down XML snippet to encapsulated joint_position_controller_
+
+ TiXmlElement *max_rate_elem = config->FirstChildElement("max_rate") ;
+ if (!max_rate_elem)
+ return false ;
+ if(max_rate_elem->QueryDoubleAttribute("value", &max_rate_) != TIXML_SUCCESS )
+ return false ;
+
+ TiXmlElement *max_acc_elem = config->FirstChildElement("max_acc") ;
+ if (!max_acc_elem)
+ return false ;
+ if(max_acc_elem->QueryDoubleAttribute("value", &max_acc_) != TIXML_SUCCESS )
+ return false ;
+
return true ;
}
@@ -79,35 +92,42 @@
{
//if (!joint_state_->calibrated_)
// return;
-
- double time = robot_->hw_->current_time_ ;
-
if (traj_lock_.trylock())
{
- const double traj_duration = traj_.getTotalTime() ;
- if (traj_duration > 1e-6) // Short trajectories make the mod_time calculation unstable
+ if (traj_duration_ > 1e-6) // Short trajectories could make the mod_time calculation unstable
{
- double time_from_start = time - traj_start_time_ ;
- double mod_time = time_from_start - floor(time_from_start/traj_.getTotalTime())*traj_.getTotalTime() ;
-
+ double profile_time = getCurProfileTime() ;
+
trajectory::Trajectory::TPoint sampled_point ;
sampled_point.dimension_ = 1 ;
sampled_point.q_.resize(1) ;
sampled_point.qdot_.resize(1) ;
int result ;
-
- result = traj_.sample(sampled_point, mod_time) ;
+
+ result = traj_.sample(sampled_point, profile_time) ;
if (result > 0)
{
joint_position_controller_.setCommand(sampled_point.q_[0]) ;
joint_position_controller_.update() ;
}
-
}
traj_lock_.unlock() ;
}
}
+double LaserScannerTrajController::getCurProfileTime()
+{
+ double time = robot_->hw_->current_time_ ;
+ double time_from_start = time - traj_start_time_ ;
+ double mod_time = time_from_start - floor(time_from_start/traj_.getTotalTime())*traj_.getTotalTime() ;
+ return mod_time ;
+}
+
+double LaserScannerTrajController::getProfileDuration()
+{
+ return traj_duration_ ;
+}
+
void LaserScannerTrajController::setTrajectory(const std::vector<trajectory::Trajectory::TPoint>& traj_points, double max_rate, double max_acc, std::string interp)
{
while (!traj_lock_.trylock())
@@ -121,122 +141,126 @@
traj_.setMaxRates(max_rates) ;
traj_.setMaxAcc(max_accs) ;
traj_.setInterpolationMethod(interp) ;
-
- //for (unsigned int i=0; i<traj_points.
-
+
traj_.setTrajectory(traj_points) ;
traj_start_time_ = robot_->hw_->current_time_ ;
-
+
+ traj_duration_ = traj_.getTotalTime() ;
+
traj_lock_.unlock() ;
}
+void LaserScannerTrajController::setPeriodicCmd(const pr2_mechanism_controllers::PeriodicCmd& cmd_)
+{
+ if (cmd_.profile == "linear" ||
+ cmd_.profile == "blended_linear")
+ {
+ double high_pt = cmd_.amplitude + cmd_.offset ;
+ double low_pt = -cmd_.amplitude + cmd_.offset ;
+
+ std::vector<trajectory::Trajectory::TPoint> tpoints ;
+
+ trajectory::Trajectory::TPoint cur_point(1) ;
+
+ cur_point.dimension_ = 1 ;
+
+ cur_point.q_[0] = low_pt ;
+ cur_point.time_ = 0.0 ;
+ tpoints.push_back(cur_point) ;
+
+ cur_point.q_[0] = high_pt ;
+ cur_point.time_ = cmd_.period/2.0 ;
+ tpoints.push_back(cur_point) ;
+
+ cur_point.q_[0] = low_pt ;
+ cur_point.time_ = cmd_.period ;
+ tpoints.push_back(cur_point) ;
+
+ setTrajectory(tpoints, max_rate_, max_acc_, cmd_.profile) ;
+ ROS_INFO("LaserScannerTrajController: Periodic Command set") ;
+ }
+ else
+ {
+ ROS_WARN("Unknown Periodic Trajectory Type. Not setting command.") ;
+ }
+}
+
ROS_REGISTER_CONTROLLER(LaserScannerTrajControllerNode)
-LaserScannerTrajControllerNode::LaserScannerTrajControllerNode(): node_(ros::node::instance())
+LaserScannerTrajControllerNode::LaserScannerTrajControllerNode(): node_(ros::node::instance()), c_()
{
- c_ = new LaserScannerTrajController();
+ need_to_send_msg_ = false ; // Haven't completed a sweep yet, so don't need to send a msg
+ publisher_ = NULL ; // We don't know our topic yet, so we can't build it
}
LaserScannerTrajControllerNode::~LaserScannerTrajControllerNode()
{
- node_->unsubscribe(service_prefix_ + "/set_command");
- delete c_;
+ node_->unsubscribe(service_prefix_ + "/set_periodic_cmd") ;
+
+ publisher_->stop() ;
+ delete publisher_ ; // Probably should wait on publish_->is_running() before exiting. Need to
+ // look into shutdown semantics for realtime_publisher
}
void LaserScannerTrajControllerNode::update()
{
- c_->update();
-}
+ c_.update() ;
+ double cur_profile_time = c_.getCurProfileTime() ;
-void LaserScannerTrajControllerNode::setCommand()
-{
- if (cmd_.data >= -.5 && cmd_.data <= .5)
+ // Check if we crossed the middle point of our profile
+ if (cur_profile_time >= c_.getProfileDuration()/2.0 &&
+ prev_profile_time_ < c_.getProfileDuration()/2.0)
{
- std::vector<trajectory::Trajectory::TPoint> tpoints ;
-
- trajectory::Trajectory::TPoint cur_point(1) ;
-
- cur_point.dimension_ = 1 ;
-
- cur_point.q_[0] = 0 ;
- cur_point.time_ = 0.0 ;
- tpoints.push_back(cur_point) ;
-
- cur_point.q_[0] = 0 ;
- cur_point.time_ = 50.0 ;
- tpoints.push_back(cur_point) ;
-
- cur_point.q_[0] = 0 ;
- cur_point.time_ = 100.0 ;
- tpoints.push_back(cur_point) ;
-
- double max_rate = 1 ;
- double max_acc = 1 ;
-
- c_->setTrajectory(tpoints, max_rate, max_acc, "linear") ;
+ // Should we be populating header.stamp here? Or, we can simply let ros take care of the timestamp
+ m_scanner_signal_.signal = 0 ;
+ need_to_send_msg_ = true ;
}
- else if (cmd_.data >= -52.1 && cmd_.data <= -51.9)
+ else if (cur_profile_time < prev_profile_time_) // Check if we wrapped around
{
- std::vector<trajectory::Trajectory::TPoint> tpoints ;
-
- trajectory::Trajectory::TPoint cur_point(1) ;
-
- cur_point.dimension_ = 1 ;
-
- cur_point.q_[0] = .5 ;
- cur_point.time_ = 0.0 ;
- tpoints.push_back(cur_point) ;
-
- cur_point.q_[0] = -.5 ;
- cur_point.time_ = 5.0 ;
- tpoints.push_back(cur_point) ;
-
- cur_point.q_[0] = .5 ;
- cur_point.time_ = 10.0 ;
- tpoints.push_back(cur_point) ;
-
- double max_rate = 1 ;
- double max_acc = 1 ;
-
- c_->setTrajectory(tpoints, max_rate, max_acc, "linear") ;
+ m_scanner_signal_.signal = 1 ;
+ need_to_send_msg_ = true ;
}
- else if (cmd_.data >= -53.1 && cmd_.data <= -52.9)
+ prev_profile_time_ = cur_profile_time ;
+
+ // Use the realtime_publisher to try to send the message.
+ // If it fails sending, it's not a big deal, since we can just try again 1 ms later. No one will notice.
+ if (need_to_send_msg_)
{
- std::vector<trajectory::Trajectory::TPoint> tpoints ;
-
- trajectory::Trajectory::TPoint cur_point(1) ;
-
- cur_point.dimension_ = 1 ;
-
- cur_point.q_[0] = .5 ;
- cur_point.time_ = 0.0 ;
- tpoints.push_back(cur_point) ;
-
- cur_point.q_[0] = -.5 ;
- cur_point.time_ = 5.0 ;
- tpoints.push_back(cur_point) ;
-
- cur_point.q_[0] = .5 ;
- cur_point.time_ = 10.0 ;
- tpoints.push_back(cur_point) ;
-
- double max_rate = 1 ;
- double max_acc = 1 ;
-
- c_->setTrajectory(tpoints, max_rate, max_acc, "blended_linear") ;
+ if (publisher_->trylock())
+ {
+ publisher_->msg_.header = m_scanner_signal_.header ;
+ publisher_->msg_.signal = m_scanner_signal_.signal ;
+ publisher_->unlockAndPublish() ;
+ need_to_send_msg_ = false ;
+ }
+ //printf("tilt_laser: Signal trigger (%u)\n", m_scanner_signal_.signal) ;
+ //std::cout << std::flush ;
}
}
bool LaserScannerTrajControllerNode::initXml(mechanism::RobotState *robot, TiXmlElement *config)
{
- service_prefix_ = config->Attribute("name");
+ service_prefix_ = config->Attribute("name") ;
- if (!c_->initXml(robot, config))
+ if (!c_.initXml(robot, config))
{
- return false;
+ ROS_ERROR("Error Loading LaserScannerTrajControllerNode XML") ;
+ return false ;
}
- node_->subscribe(service_prefix_ + "/set_command", cmd_, &LaserScannerTrajControllerNode::setCommand, this, 1);
- return true;
+ node_->subscribe(service_prefix_ + "/set_periodic_cmd", cmd_, &LaserScannerTrajControllerNode::setPeriodicCmd, this, 1) ;
+
+ if (publisher_ != NULL) // Make sure that we don't memory leak if initXml gets called twice
+ delete publisher_ ;
+ publisher_ = new misc_utils::RealtimePublisher <pr2_mechanism_controllers::LaserScannerSignal> (service_prefix_ + "/laser_scanner_signal", 1) ;
+
+ prev_profile_time_ = 0.0 ;
+
+ return true ;
}
+
+void LaserScannerTrajControllerNode::setPeriodicCmd()
+{
+ c_.setPeriodicCmd(cmd_) ;
+}
Copied: pkg/trunk/robot_descriptions/pr2/pr2_experimental_controllers/laser_tilt/laser_tilt_controller.xml (from rev 8179, pkg/trunk/robot_descriptions/wg_robot_description/tilt_laser_deprecated/tilt_laser_traj_controller.xml)
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_experimental_controllers/laser_tilt/laser_tilt_controller.xml (rev 0)
+++ pkg/trunk/robot_descriptions/pr2/pr2_experimental_controllers/laser_tilt/laser_tilt_controller.xml 2008-12-16 18:44:27 UTC (rev 8180)
@@ -0,0 +1,10 @@
+<controllers>
+ <controller name="laser_controller" topic="laser_controller" type="LaserScannerTrajControllerNode">
+ <max_acc value="1.0" />
+ <max_rate value="100.0" />
+ <filter smoothing_factor="0.1" />
+ <joint name="laser_tilt_mount_joint">
+ <pid p="12" i=".1" d="1" iClamp="0.5" />
+ </joint>
+ </controller>
+</controllers>
Deleted: pkg/trunk/robot_descriptions/wg_robot_description/tilt_laser_deprecated/tilt_laser_traj_controller.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/tilt_laser_deprecated/tilt_laser_traj_controller.xml 2008-12-16 18:36:11 UTC (rev 8179)
+++ pkg/trunk/robot_descriptions/wg_robot_description/tilt_laser_deprecated/tilt_laser_traj_controller.xml 2008-12-16 18:44:27 UTC (rev 8180)
@@ -1,8 +0,0 @@
-<controllers>
- <controller name="laser_controller" topic="laser_controller" type="LaserScannerTrajControllerNode">
- <filter smoothing_factor="0.1" />
- <joint name="laser_tilt_mount_joint">
- <pid p="12" i=".1" d="1" iClamp="0.5" />
- </joint>
- </controller>
-</controllers>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
