simulator_art: simulator_art: vehicle

00001 /*
00002  *  Copyright (C) 2005, 2007, 2009 Austin Robot Technology
00003  *
00004  *  License: Modified BSD Software License Agreement
00005  * 
00006  *  $Id: vehicle_model.cc 698 2010-10-25 15:27:01Z jack.oquin $
00007  */
00008 
00017 #include <angles/angles.h>
00018 
00019 #include <art/conversions.h>
00020 #include <art/frames.h>
00021 #include <art/UTM.h>
00022 
00023 #include <art_msgs/BrakeState.h>
00024 #include <art_msgs/Shifter.h>
00025 #include <art_msgs/SteeringState.h>
00026 #include <art_msgs/ThrottleState.h>
00027 #include <art/steering.h>
00028 
00029 #include <art_msgs/GpsInfo.h>
00030 
00031 #include "vehicle_model.h"
00032 
00033 void ArtVehicleModel::setup(void)
00034 {
00035   int qDepth = 1;
00036   ros::TransportHints noDelay = ros::TransportHints().tcpNoDelay(true);
00037 
00038   odom_pub_ =
00039     node_.advertise<nav_msgs::Odometry>(ns_prefix_ + "odom", qDepth);
00040   ground_truth_pub_ =
00041     node_.advertise<nav_msgs::Odometry>(ns_prefix_ + "ground_truth", qDepth);
00042   imu_pub_ =
00043     node_.advertise<sensor_msgs::Imu>(ns_prefix_ + "imu", qDepth);
00044   gps_pub_ =
00045     node_.advertise<art_msgs::GpsInfo>(ns_prefix_ + "gps", qDepth);
00046   
00047   // servo state topics
00048   brake_sub_ =
00049     node_.subscribe(ns_prefix_ + "brake/state", qDepth,
00050                     &ArtVehicleModel::brakeReceived, this, noDelay);
00051   shifter_sub_ =
00052     node_.subscribe(ns_prefix_ + "shifter/state", qDepth,
00053                     &ArtVehicleModel::shifterReceived, this, noDelay);
00054   steering_sub_ =
00055     node_.subscribe(ns_prefix_ + "steering/state", qDepth,
00056                     &ArtVehicleModel::steeringReceived, this, noDelay);
00057   throttle_sub_ =
00058     node_.subscribe(ns_prefix_ + "throttle/state", qDepth,
00059                     &ArtVehicleModel::throttleReceived, this, noDelay);
00060 
00061   // set default GPS origin, from SwRI site visit in San Antonio
00062   ros::NodeHandle private_nh("~");
00063   private_nh.param("latitude",  origin_lat_,   29.446018);
00064   private_nh.param("longitude", origin_long_, -98.607024);
00065   private_nh.param("elevation", origin_elev_, 100.0);
00066   ROS_INFO("map GPS origin: latitude %.6f, longitude %.6f, elevation %.1f m",
00067            origin_lat_, origin_long_, origin_elev_);
00068 
00069   // Convert latitude and longitude of map origin to UTM.
00070   UTM::LLtoUTM(origin_lat_, origin_long_,
00071                origin_northing_, origin_easting_, origin_zone_);
00072   ROS_INFO("Map origin UTM: %s, %.2f, %.2f",
00073            origin_zone_, origin_easting_, origin_northing_);
00074 
00075   // Round UTM origin of map to nearest UTM grid intersection.
00076   double grid_x = (rint(origin_easting_/UTM::grid_size) * UTM::grid_size);
00077   double grid_y = (rint(origin_northing_/UTM::grid_size) * UTM::grid_size);
00078   ROS_INFO("UTM grid origin: (%.f, %.f)", grid_x , grid_y);
00079 
00080   // Report stage-generated odometry relative to that location.
00081   map_origin_x_ = origin_easting_ - grid_x;
00082   map_origin_y_ = origin_northing_ - grid_y;
00083   ROS_INFO("MapXY origin: (%.f, %.f)", map_origin_x_ , map_origin_y_);
00084 }
00085 
00086 // Servo device interfaces.
00087 //
00088 // IMPORTANT: These callbacks run in a separate thread, so all class
00089 // data updates must be done while holding the msg_lock_.
00090 //
00091 void
00092 ArtVehicleModel::brakeReceived(const art_msgs::BrakeState::ConstPtr &msg)
00093 {
00094   //ROS_DEBUG("brake state received: position %.3f", msg->position);
00095   boost::mutex::scoped_lock lock(msg_lock_);
00096   brake_position_ = msg->position;
00097 }
00098 
00099 void
00100 ArtVehicleModel::shifterReceived(const art_msgs::Shifter::ConstPtr &msg)
00101 {
00102   ROS_DEBUG("shifter state received: gear %u", msg->gear);
00103   boost::mutex::scoped_lock lock(msg_lock_);
00104   shifter_gear_ = msg->gear;
00105 }
00106 
00107 void 
00108 ArtVehicleModel::steeringReceived(const art_msgs::SteeringState::ConstPtr &msg)
00109 {
00110   //ROS_DEBUG("steering state received: %.1f (degrees)", msg->angle);
00111   boost::mutex::scoped_lock lock(msg_lock_);
00112   steering_angle_ = msg->angle;
00113 }
00114 
00115 void
00116 ArtVehicleModel::throttleReceived(const art_msgs::ThrottleState::ConstPtr &msg)
00117 {
00118   //ROS_DEBUG("throttle state received: position %.3f", msg->position);
00119   boost::mutex::scoped_lock lock(msg_lock_);
00120   throttle_position_ = msg->position;
00121 }
00122 
00137 void ArtVehicleModel::ModelAcceleration(geometry_msgs::Twist *odomVel,
00138                                         sensor_msgs::Imu *imuMsg,
00139                                         ros::Time sim_time)
00140 {
00141   // MUST serialize with updates from incoming messages
00142   boost::mutex::scoped_lock lock(msg_lock_);
00143 
00144   double speed = fabs(odomVel->linear.x);
00145 
00146   // assume full brake or throttle produces 1g (9.81 m/s/s)
00147   // TODO: tune these coefficients using actual vehicle measurements
00148   static const double g = 9.81;         // acceleration due to gravity
00149   static const double throttle_accel = g;
00150   static const double brake_decel = g;
00151   static const double rolling_resistance = 0.01 * g;
00152   static const double drag_coeff = 0.01;
00153 
00154   // the vehicle idles at 7 MPH (3.1 m/s) with no brake or throttle
00155   static const double idle_accel = (rolling_resistance
00156                                     + drag_coeff * 3.1 * 3.1);
00157 
00158   double wind_resistance = drag_coeff * speed * speed;
00159   double accel = (idle_accel
00160                   + throttle_position_ * throttle_accel
00161                   - brake_position_ * brake_decel
00162                   - rolling_resistance
00163                   - wind_resistance);
00164 
00165   // compute seconds since last update (probably zero first time)
00166   double deltaT = ros::Duration(sim_time - last_update_time_).toSec();
00167   speed += accel * deltaT;              // adjust speed
00168   imuMsg->linear_acceleration.x = accel;
00169 
00170   // Brake and throttle (by themselves) never cause reverse motion.
00171   // Only shifting into Reverse can do that.
00172   if (speed < 0.0)
00173     {
00174       speed = 0.0;
00175       imuMsg->linear_acceleration.x = 0.0;
00176     }
00177 
00178   // Set velocity sign based on gear.
00179   odomVel->linear.x = speed;            // forward movement
00180   if (shifter_gear_ ==  art_msgs::Shifter::Reverse)
00181     odomVel->linear.x = -speed;         // reverse movement
00182 
00183   // set yaw rate (radians/second) from velocity and steering angle
00184   odomVel->angular.z = Steering::angle_to_yaw(odomVel->linear.x,
00185                                               steering_angle_);
00186   imuMsg->angular_velocity.z = odomVel->angular.z;
00187 
00188   // set simulated vehicle velocity using the "car" steering model,
00189   // which uses steering angle in radians instead of yaw rate.
00190   double angleRadians = angles::from_degrees(steering_angle_);
00191   ROS_DEBUG("Stage SetSpeed(%.3f, %.3f, %.3f)",
00192             odomVel->linear.x, odomVel->linear.y, angleRadians);
00193   stgp_->SetSpeed(odomVel->linear.x, odomVel->linear.y, angleRadians);
00194 }
00195 
00196 // update vehicle dynamics model
00197 void ArtVehicleModel::update(ros::Time sim_time)
00198 {
00199   sensor_msgs::Imu imu_msg;
00200 
00201   // model vehicle acceleration from servo actuators
00202   ModelAcceleration(&odomMsg_.twist.twist, &imu_msg, sim_time);
00203 
00204   // Get latest position data from Stage
00205   // Translate into ROS message format and publish
00206   odomMsg_.pose.pose.position.x = stgp_->est_pose.x + map_origin_x_;
00207   odomMsg_.pose.pose.position.y = stgp_->est_pose.y + map_origin_y_;
00208   odomMsg_.pose.pose.position.z = origin_elev_;
00209   odomMsg_.pose.pose.orientation =
00210     tf::createQuaternionMsgFromYaw(stgp_->est_pose.a);
00211 
00212   odomMsg_.header.stamp = sim_time;
00213   odomMsg_.header.frame_id = tf_prefix_ + ArtFrames::earth;
00214   odomMsg_.child_frame_id = tf_prefix_ + ArtFrames::vehicle;
00215   odom_pub_.publish(odomMsg_);
00216 
00217   // publish simulated IMU data
00218   imu_msg.header.stamp = sim_time;
00219   imu_msg.header.frame_id = tf_prefix_ + ArtFrames::vehicle;
00220   imu_msg.orientation = odomMsg_.pose.pose.orientation;
00221   imu_pub_.publish(imu_msg);
00222 
00223   // broadcast /earth transform relative to sea level
00224   tf::Quaternion vehicleQ;
00225   tf::quaternionMsgToTF(odomMsg_.pose.pose.orientation, vehicleQ);
00226   tf::Transform txEarth(vehicleQ, 
00227                        tf::Point(odomMsg_.pose.pose.position.x,
00228                                  odomMsg_.pose.pose.position.y,
00229                                  odomMsg_.pose.pose.position.z));
00230   tf_->sendTransform(tf::StampedTransform(txEarth, sim_time,
00231                                           tf_prefix_ + ArtFrames::earth,
00232                                           tf_prefix_ + ArtFrames::vehicle));
00233 
00234   // Also publish /odom frame with same elevation as /vehicle and same
00235   // orientation as /earth
00236   tf::Transform txOdom(tf::Quaternion(0.0, 0.0, 0.0, 1.0), 
00237                        tf::Point(0.0, 0.0, -odomMsg_.pose.pose.position.z));
00238   tf_->sendTransform(tf::StampedTransform(txOdom, sim_time,
00239                                           tf_prefix_ + ArtFrames::odom,
00240                                           tf_prefix_ + ArtFrames::earth));
00241 
00242   // Also publish the ground truth pose and velocity, correcting for
00243   // Stage's screwed-up coord system.
00244   Stg::Pose gpose = stgp_->GetGlobalPose();
00245   Stg::Velocity gvel = stgp_->GetGlobalVelocity();
00246   tf::Quaternion gposeQ;
00247   gposeQ.setRPY(0.0, 0.0, gpose.a-M_PI/2.0);
00248   tf::Transform gt(gposeQ, tf::Point(gpose.y, -gpose.x, 0.0));
00249   tf::Quaternion gvelQ;
00250   gvelQ.setRPY(0.0, 0.0, gvel.a-M_PI/2.0);
00251   tf::Transform gv(gvelQ, tf::Point(gvel.y, -gvel.x, 0.0));
00252 
00253   groundTruthMsg_.pose.pose.position.x     = gt.getOrigin().x();
00254   groundTruthMsg_.pose.pose.position.y     = gt.getOrigin().y();
00255   groundTruthMsg_.pose.pose.position.z     = gt.getOrigin().z();
00256   groundTruthMsg_.pose.pose.orientation.x  = gt.getRotation().x();
00257   groundTruthMsg_.pose.pose.orientation.y  = gt.getRotation().y();
00258   groundTruthMsg_.pose.pose.orientation.z  = gt.getRotation().z();
00259   groundTruthMsg_.pose.pose.orientation.w  = gt.getRotation().w();
00260   groundTruthMsg_.twist.twist.linear.x     = gv.getOrigin().x();
00261   groundTruthMsg_.twist.twist.linear.y     = gv.getOrigin().y();
00262   groundTruthMsg_.twist.twist.angular.z    = gvel.a;
00263   groundTruthMsg_.header.stamp = sim_time;
00264   groundTruthMsg_.header.frame_id = tf_prefix_ + ArtFrames::odom;
00265   groundTruthMsg_.child_frame_id = tf_prefix_ + ArtFrames::vehicle;
00266   ground_truth_pub_.publish(groundTruthMsg_);
00267 
00268   publishGPS(sim_time);
00269 
00270   last_update_time_ = sim_time;
00271 }
00272 
00273 void ArtVehicleModel::publishGPS(ros::Time sim_time)
00274 {
00275   art_msgs::GpsInfo gpsi;
00276 
00277   gpsi.header.stamp = sim_time;
00278   gpsi.header.frame_id = tf_prefix_ + ArtFrames::odom;
00279 
00280   // relocate pose relative to map origin
00281   gpsi.utm_e = (odomMsg_.pose.pose.position.x
00282                 - map_origin_x_ + origin_easting_);
00283   gpsi.utm_n = (odomMsg_.pose.pose.position.y
00284                 - map_origin_y_ + origin_northing_);
00285 
00286   UTM::UTMtoLL(gpsi.utm_n, gpsi.utm_e, origin_zone_,
00287                gpsi.latitude, gpsi.longitude);
00288 
00289   gpsi.zone = origin_zone_;
00290   gpsi.altitude   = odomMsg_.pose.pose.position.z;
00291   gpsi.quality = art_msgs::GpsInfo::DGPS_FIX;
00292   gpsi.num_sats = 9;
00293 
00294   gps_pub_.publish(gpsi);
00295 }