mvsim_node.cpp
Go to the documentation of this file.
00001 
00004 #include "mvsim/mvsim_node_core.h"
00005 #include <rapidxml_utils.hpp>
00006 #include <iostream>
00007 
00008 #include <mrpt/system/os.h>  // kbhit()
00009 #include <mrpt/system/filesystem.h>
00010 
00011 #include <mrpt_bridge/laser_scan.h>
00012 #include <mrpt_bridge/map.h>
00013 #include <mrpt_bridge/pose.h>
00014 
00015 #include <nav_msgs/MapMetaData.h>
00016 #include <nav_msgs/GetMap.h>
00017 #include <geometry_msgs/PoseWithCovarianceStamped.h>
00018 #include <geometry_msgs/PoseArray.h>
00019 #include <geometry_msgs/Polygon.h>
00020 #include <sensor_msgs/LaserScan.h>
00021 
00022 #include <nav_msgs/Odometry.h>
00023 #include <mvsim/WorldElements/OccupancyGridMap.h>
00024 
00025 /*------------------------------------------------------------------------------
00026  * MVSimNode()
00027  * Constructor.
00028  *----------------------------------------------------------------------------*/
00029 MVSimNode::MVSimNode(ros::NodeHandle& n)
00030         : mvsim_world_(*this),
00031           realtime_factor_(1.0),
00032           gui_refresh_period_ms_(75),
00033           m_show_gui(true),
00034           m_do_fake_localization(true),
00035           m_transform_tolerance(0.1),
00036           m_n(n),
00037           m_localn("~"),
00038           m_tf_br(),
00039           m_tfIdentity(tf::createIdentityQuaternion(), tf::Point(0, 0, 0)),
00040           t_old_(-1),
00041           world_init_ok_(false),
00042           m_period_ms_publish_tf(20),
00043           m_period_ms_teleop_refresh(100),
00044           m_teleop_idx_veh(0)
00045 {
00046         // Launch GUI thread:
00047         thread_params_.obj = this;
00048         thGUI_ =
00049                 std::thread(&MVSimNode::thread_update_GUI, std::ref(thread_params_));
00050 
00051         // Init ROS publishers:
00052         m_pub_clock = m_n.advertise<rosgraph_msgs::Clock>("/clock", 10);
00053 
00054         m_pub_map_ros = m_n.advertise<nav_msgs::OccupancyGrid>(
00055                 "simul_map", 1 /*queue len*/, true /*latch*/);
00056         m_pub_map_metadata = m_n.advertise<nav_msgs::MapMetaData>(
00057                 "simul_map_metadata", 1 /*queue len*/, true /*latch*/);
00058 
00059         m_sim_time.fromSec(0.0);
00060         m_base_last_cmd.fromSec(0.0);
00061 
00062         // Node parameters:
00063         double t;
00064         if (!m_localn.getParam("base_watchdog_timeout", t)) t = 0.2;
00065         m_base_watchdog_timeout.fromSec(t);
00066 
00067         m_localn.param("realtime_factor", realtime_factor_, 1.0);
00068         m_localn.param(
00069                 "gui_refresh_period", gui_refresh_period_ms_, gui_refresh_period_ms_);
00070         m_localn.param("show_gui", m_show_gui, m_show_gui);
00071         m_localn.param(
00072                 "period_ms_publish_tf", m_period_ms_publish_tf, m_period_ms_publish_tf);
00073         m_localn.param(
00074                 "do_fake_localization", m_do_fake_localization, m_do_fake_localization);
00075 
00076         // In case the user didn't set it:
00077         m_n.setParam("/use_sim_time", true);
00078 }
00079 
00080 void MVSimNode::loadWorldModel(const std::string& world_xml_file)
00081 {
00082         ROS_INFO("[MVSimNode] Loading world file: %s", world_xml_file.c_str());
00083         ROS_ASSERT_MSG(
00084                 mrpt::system::fileExists(world_xml_file),
00085                 "[MVSimNode::loadWorldModel] File does not exist!: '%s'",
00086                 world_xml_file.c_str());
00087 
00088         // Load from XML:
00089         rapidxml::file<> fil_xml(world_xml_file.c_str());
00090         mvsim_world_.load_from_XML(fil_xml.data(), world_xml_file);
00091 
00092         ROS_INFO("[MVSimNode] World file load done.");
00093         world_init_ok_ = true;
00094 
00095         // Notify the ROS system about the good news:
00096         notifyROSWorldIsUpdated();
00097 }
00098 
00099 /*------------------------------------------------------------------------------
00100  * ~MVSimNode()
00101  * Destructor.
00102  *----------------------------------------------------------------------------*/
00103 MVSimNode::~MVSimNode()
00104 {
00105         thread_params_.closing = true;
00106         thGUI_.join();
00107 }
00108 
00109 /*------------------------------------------------------------------------------
00110  * configCallback()
00111  * Callback function for dynamic reconfigure server.
00112  *----------------------------------------------------------------------------*/
00113 void MVSimNode::configCallback(mvsim::mvsimNodeConfig& config, uint32_t level)
00114 {
00115         // Set class variables to new values. They should match what is input at the
00116         // dynamic reconfigure GUI.
00117         //  message = config.message.c_str();
00118         ROS_INFO("MVSimNode::configCallback() called.");
00119 
00120         if (mvsim_world_.is_GUI_open() && !config.show_gui)
00121                 mvsim_world_.close_GUI();
00122 }
00123 
00124 // Process pending msgs, run real-time simulation, etc.
00125 void MVSimNode::spin()
00126 {
00127         using namespace mvsim;
00128 
00129         // Do simulation itself:
00130         // ========================================================================
00131         // Handle 1st iter:
00132         if (t_old_ < 0) t_old_ = realtime_tictac_.Tac();
00133         // Compute how much time has passed to simulate in real-time:
00134         double t_new = realtime_tictac_.Tac();
00135         double incr_time = realtime_factor_ * (t_new - t_old_);
00136 
00137         if (incr_time < mvsim_world_.get_simul_timestep())  // Just in case the
00138                                                                                                                 // computer is *really
00139                                                                                                                 // fast*...
00140                 return;
00141 
00142         // Simulate:
00143         mvsim_world_.run_simulation(incr_time);
00144 
00145         // t_old_simul = world.get_simul_time();
00146         t_old_ = t_new;
00147 
00148         const World::TListVehicles& vehs = mvsim_world_.getListOfVehicles();
00149 
00150         // Publish new state to ROS
00151         // ========================================================================
00152         this->spinNotifyROS();
00153 
00154         // GUI msgs, teleop, etc.
00155         // ========================================================================
00156         if (m_tim_teleop_refresh.Tac() > m_period_ms_teleop_refresh * 1e-3)
00157         {
00158                 m_tim_teleop_refresh.Tic();
00159 
00160                 std::string txt2gui_tmp;
00161                 World::TGUIKeyEvent keyevent = m_gui_key_events;
00162 
00163                 // Global keys:
00164                 switch (keyevent.keycode)
00165                 {
00166                         // case 27: do_exit=true; break;
00167                         case '1':
00168                         case '2':
00169                         case '3':
00170                         case '4':
00171                         case '5':
00172                         case '6':
00173                                 m_teleop_idx_veh = keyevent.keycode - '1';
00174                                 break;
00175                 };
00176 
00177                 {  // Test: Differential drive: Control raw forces
00178                         txt2gui_tmp += mrpt::format(
00179                                 "Selected vehicle: %u/%u\n",
00180                                 static_cast<unsigned>(m_teleop_idx_veh + 1),
00181                                 static_cast<unsigned>(vehs.size()));
00182                         if (vehs.size() > m_teleop_idx_veh)
00183                         {
00184                                 // Get iterator to selected vehicle:
00185                                 World::TListVehicles::const_iterator it_veh = vehs.begin();
00186                                 std::advance(it_veh, m_teleop_idx_veh);
00187 
00188                                 // Get speed: ground truth
00189                                 {
00190                                         const vec3& vel = it_veh->second->getVelocityLocal();
00191                                         txt2gui_tmp += mrpt::format(
00192                                                 "gt. vel: lx=%7.03f, ly=%7.03f, w= %7.03fdeg/s\n",
00193                                                 vel.vals[0], vel.vals[1],
00194                                                 mrpt::utils::RAD2DEG(vel.vals[2]));
00195                                 }
00196                                 // Get speed: ground truth
00197                                 {
00198                                         const vec3& vel =
00199                                                 it_veh->second->getVelocityLocalOdoEstimate();
00200                                         txt2gui_tmp += mrpt::format(
00201                                                 "odo vel: lx=%7.03f, ly=%7.03f, w= %7.03fdeg/s\n",
00202                                                 vel.vals[0], vel.vals[1],
00203                                                 mrpt::utils::RAD2DEG(vel.vals[2]));
00204                                 }
00205 
00206                                 // Generic teleoperation interface for any controller that
00207                                 // supports it:
00208                                 {
00209                                         ControllerBaseInterface* controller =
00210                                                 it_veh->second->getControllerInterface();
00211                                         ControllerBaseInterface::TeleopInput teleop_in;
00212                                         ControllerBaseInterface::TeleopOutput teleop_out;
00213                                         teleop_in.keycode = keyevent.keycode;
00214                                         controller->teleop_interface(teleop_in, teleop_out);
00215                                         txt2gui_tmp += teleop_out.append_gui_lines;
00216                                 }
00217                         }
00218                 }
00219 
00220                 m_msg2gui = txt2gui_tmp;  // send txt msgs to show in the GUI
00221 
00222                 // Clear the keystroke buffer
00223                 if (keyevent.keycode != 0) m_gui_key_events = World::TGUIKeyEvent();
00224 
00225         }  // end refresh teleop stuff
00226 }
00227 
00228 /*------------------------------------------------------------------------------
00229  * thread_update_GUI()
00230  *----------------------------------------------------------------------------*/
00231 void MVSimNode::thread_update_GUI(TThreadParams& thread_params)
00232 {
00233         using namespace mvsim;
00234 
00235         MVSimNode* obj = thread_params.obj;
00236 
00237         while (!thread_params.closing)
00238         {
00239                 if (obj->world_init_ok_ && obj->m_show_gui)
00240                 {
00241                         World::TUpdateGUIParams guiparams;
00242                         guiparams.msg_lines = obj->m_msg2gui;
00243 
00244                         obj->mvsim_world_.update_GUI(&guiparams);
00245 
00246                         // Send key-strokes to the main thread:
00247                         if (guiparams.keyevent.keycode != 0)
00248                                 obj->m_gui_key_events = guiparams.keyevent;
00249                 }
00250                 std::this_thread::sleep_for(
00251                         std::chrono::milliseconds(obj->gui_refresh_period_ms_));
00252         }
00253 }
00254 
00255 // Visitor: Vehicles
00256 // ----------------------------------------
00257 void MVSimNode::MVSimVisitor_notifyROSWorldIsUpdated::visit(
00258         mvsim::VehicleBase* obj)
00259 {
00260 }  // end visit(Vehicles)
00261 
00262 // Visitor: World elements
00263 // ----------------------------------------
00264 void MVSimNode::MVSimVisitor_notifyROSWorldIsUpdated::visit(
00265         mvsim::WorldElementBase* obj)
00266 {
00267         // GridMaps --------------
00268         if (dynamic_cast<mvsim::OccupancyGridMap*>(obj))
00269         {
00270                 mvsim::OccupancyGridMap* grid =
00271                         dynamic_cast<mvsim::OccupancyGridMap*>(obj);
00272 
00273                 nav_msgs::OccupancyGrid ros_map;
00274                 mrpt_bridge::convert(grid->getOccGrid(), ros_map);
00275 
00276                 static size_t loop_count = 0;
00277                 ros_map.header.stamp = ros::Time::now();
00278                 ros_map.header.seq = loop_count++;
00279 
00280                 m_parent.m_pub_map_ros.publish(ros_map);
00281                 m_parent.m_pub_map_metadata.publish(ros_map.info);
00282 
00283         }  // end gridmap
00284 
00285 }  // end visit(World Elements)
00286 
00287 // ROS: Publish grid map for visualization purposes:
00288 void MVSimNode::notifyROSWorldIsUpdated()
00289 {
00290         MVSimVisitor_notifyROSWorldIsUpdated myvisitor(*this);
00291 
00292         mvsim_world_.runVisitorOnWorldElements(myvisitor);
00293         mvsim_world_.runVisitorOnVehicles(myvisitor);
00294 
00295         // Create subscribers & publishers for each vehicle's stuff:
00296         // ----------------------------------------------------
00297         mvsim::World::TListVehicles& vehs = mvsim_world_.getListOfVehicles();
00298         m_pubsub_vehicles.clear();
00299         m_pubsub_vehicles.resize(vehs.size());
00300         size_t idx = 0;
00301         for (mvsim::World::TListVehicles::iterator it = vehs.begin();
00302                  it != vehs.end(); ++it, ++idx)
00303         {
00304                 mvsim::VehicleBase* veh = it->second;
00305                 initPubSubs(m_pubsub_vehicles[idx], veh);
00306         }
00307 
00308         // Publish the static transform /world -> /map
00309         sendStaticTF("/world", "/map", m_tfIdentity, m_sim_time);
00310 }
00311 
00312 void MVSimNode::sendStaticTF(
00313         const std::string& frame_id, const std::string& child_frame_id,
00314         const tf::Transform& txf, const ros::Time& stamp)
00315 {
00316         geometry_msgs::TransformStamped tx;
00317         tx.header.frame_id = frame_id;
00318         tx.child_frame_id = child_frame_id;
00319         tx.header.stamp = stamp;
00320         tf::transformTFToMsg(txf, tx.transform);
00321         m_static_tf_br.sendTransform(tx);
00322 }
00323 
00326 void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh)
00327 {
00328         // sub: <VEH>/cmd_vel
00329         pubsubs.sub_cmd_vel = m_n.subscribe<geometry_msgs::Twist>(
00330                 vehVarName("cmd_vel", veh), 10,
00331                 boost::bind(&MVSimNode::onROSMsgCmdVel, this, _1, veh));
00332 
00333         // pub: <VEH>/odom
00334         pubsubs.pub_odom =
00335                 m_n.advertise<nav_msgs::Odometry>(vehVarName("odom", veh), 10);
00336 
00337         // pub: <VEH>/base_pose_ground_truth
00338         pubsubs.pub_ground_truth = m_n.advertise<nav_msgs::Odometry>(
00339                 vehVarName("base_pose_ground_truth", veh), 10);
00340 
00341         // pub: <VEH>/chassis_markers
00342         {
00343                 pubsubs.pub_chassis_markers =
00344                         m_n.advertise<visualization_msgs::MarkerArray>(
00345                                 vehVarName("chassis_markers", veh), 5, true /*latch*/);
00346                 const mrpt::math::TPolygon2D& poly = veh->getChassisShape();
00347 
00348                 // Create one "ROS marker" for each wheel + 1 for the chassis:
00349                 visualization_msgs::MarkerArray& msg_shapes = pubsubs.chassis_shape_msg;
00350                 msg_shapes.markers.resize(1 + veh->getNumWheels());
00351 
00352                 // [0] Chassis shape:
00353                 visualization_msgs::Marker& chassis_shape_msg = msg_shapes.markers[0];
00354 
00355                 chassis_shape_msg.action = visualization_msgs::Marker::MODIFY;
00356                 chassis_shape_msg.type = visualization_msgs::Marker::LINE_STRIP;
00357                 chassis_shape_msg.header.frame_id = vehVarName("base_link", veh);
00358                 chassis_shape_msg.ns = "mvsim.chassis_shape";
00359                 chassis_shape_msg.id = veh->getVehicleIndex();
00360                 chassis_shape_msg.scale.x = 0.05;
00361                 chassis_shape_msg.scale.y = 0.05;
00362                 chassis_shape_msg.scale.z = 0.02;
00363                 chassis_shape_msg.points.resize(poly.size() + 1);
00364                 for (size_t i = 0; i <= poly.size(); i++)
00365                 {
00366                         size_t k = i % poly.size();
00367                         chassis_shape_msg.points[i].x = poly[k].x;
00368                         chassis_shape_msg.points[i].y = poly[k].y;
00369                         chassis_shape_msg.points[i].z = 0;
00370                 }
00371                 chassis_shape_msg.color.a = 0.9;
00372                 chassis_shape_msg.color.r = 0.9;
00373                 chassis_shape_msg.color.g = 0.9;
00374                 chassis_shape_msg.color.b = 0.9;
00375                 chassis_shape_msg.frame_locked = true;
00376 
00377                 // [1:N] Wheel shapes
00378                 for (size_t i = 0; i < veh->getNumWheels(); i++)
00379                 {
00380                         visualization_msgs::Marker& wheel_shape_msg =
00381                                 msg_shapes.markers[1 + i];
00382                         const mvsim::Wheel& w = veh->getWheelInfo(i);
00383 
00384                         const double lx = w.diameter * 0.5, ly = w.width * 0.5;
00385                         wheel_shape_msg = chassis_shape_msg;  // Init values
00386                         chassis_shape_msg.ns = mrpt::format(
00387                                 "mvsim.chassis_shape.wheel%u", static_cast<unsigned int>(i));
00388                         wheel_shape_msg.points.resize(5);
00389                         wheel_shape_msg.points[0].x = lx;
00390                         wheel_shape_msg.points[0].y = ly;
00391                         wheel_shape_msg.points[0].z = 0;
00392                         wheel_shape_msg.points[1].x = lx;
00393                         wheel_shape_msg.points[1].y = -ly;
00394                         wheel_shape_msg.points[1].z = 0;
00395                         wheel_shape_msg.points[2].x = -lx;
00396                         wheel_shape_msg.points[2].y = -ly;
00397                         wheel_shape_msg.points[2].z = 0;
00398                         wheel_shape_msg.points[3].x = -lx;
00399                         wheel_shape_msg.points[3].y = ly;
00400                         wheel_shape_msg.points[3].z = 0;
00401                         wheel_shape_msg.points[4] = wheel_shape_msg.points[0];
00402 
00403                         wheel_shape_msg.color.r = w.color.R / 255.0;
00404                         wheel_shape_msg.color.g = w.color.G / 255.0;
00405                         wheel_shape_msg.color.b = w.color.B / 255.0;
00406 
00407                         // Set local pose of the wheel wrt the vehicle:
00408                         tf::Matrix3x3 rot;
00409                         rot.setEulerYPR(w.yaw, 0, 0);
00410                         tf::poseTFToMsg(
00411                                 tf::Transform(rot, tf::Vector3(w.x, w.y, 0)),
00412                                 wheel_shape_msg.pose);
00413                 }  // end for each wheel
00414 
00415                 // Publish Initial pose
00416                 pubsubs.pub_chassis_markers.publish(msg_shapes);
00417         }
00418 
00419         // pub: <VEH>/chassis_polygon
00420         {
00421                 pubsubs.pub_chassis_shape = m_n.advertise<geometry_msgs::Polygon>(
00422                         vehVarName("chassis_polygon", veh), 1, true /*latch*/);
00423 
00424                 // Do the first (and unique) publish:
00425                 geometry_msgs::Polygon poly_msg;
00426                 const mrpt::math::TPolygon2D& poly = veh->getChassisShape();
00427                 poly_msg.points.resize(poly.size());
00428                 for (size_t i = 0; i < poly.size(); i++)
00429                 {
00430                         poly_msg.points[i].x = poly[i].x;
00431                         poly_msg.points[i].y = poly[i].y;
00432                         poly_msg.points[i].z = 0;
00433                 }
00434                 pubsubs.pub_chassis_shape.publish(poly_msg);
00435         }
00436 
00437         if (m_do_fake_localization)
00438         {
00439                 // pub: <VEH>/amcl_pose
00440                 pubsubs.pub_amcl_pose =
00441                         m_n.advertise<geometry_msgs::PoseWithCovarianceStamped>(
00442                                 vehVarName("amcl_pose", veh), 1);
00443                 // pub: <VEH>/particlecloud
00444                 pubsubs.pub_particlecloud = m_n.advertise<geometry_msgs::PoseArray>(
00445                         vehVarName("particlecloud", veh), 1);
00446         }
00447 
00448         // STATIC Identity transform <VEH>/base_link -> <VEH>/base_footprint
00449         sendStaticTF(
00450                 vehVarName("base_link", veh), vehVarName("base_footprint", veh),
00451                 m_tfIdentity, m_sim_time);
00452 }
00453 
00454 void MVSimNode::onROSMsgCmdVel(
00455         const geometry_msgs::Twist::ConstPtr& cmd, mvsim::VehicleBase* veh)
00456 {
00457         mvsim::ControllerBaseInterface* controller = veh->getControllerInterface();
00458 
00459         const bool ctrlAcceptTwist =
00460                 controller->setTwistCommand(cmd->linear.x, cmd->angular.z);
00461 
00462         if (!ctrlAcceptTwist)
00463         {
00464                 ROS_DEBUG_THROTTLE(
00465                         5.0,
00466                         "*Warning* Vehicle's controller ['%s'] refuses Twist commands!",
00467                         veh->getName().c_str());
00468         }
00469 }
00470 
00472 void MVSimNode::spinNotifyROS()
00473 {
00474         using namespace mvsim;
00475         const World::TListVehicles& vehs = mvsim_world_.getListOfVehicles();
00476 
00477         // Get current simulation time (for messages) and publish "/clock"
00478         // ----------------------------------------------------------------
00479         m_sim_time.fromSec(mvsim_world_.get_simul_time());
00480         m_clockMsg.clock = m_sim_time;
00481         m_pub_clock.publish(m_clockMsg);
00482 
00483         // Publish all TFs for each vehicle:
00484         // ---------------------------------------------------------------------
00485         if (m_tim_publish_tf.Tac() > m_period_ms_publish_tf * 1e-3)
00486         {
00487                 m_tim_publish_tf.Tic();
00488 
00489                 size_t i = 0;
00490                 ROS_ASSERT(m_pubsub_vehicles.size() == vehs.size());
00491 
00492                 for (World::TListVehicles::const_iterator it = vehs.begin();
00493                          it != vehs.end(); ++it, ++i)
00494                 {
00495                         const VehicleBase* veh = it->second;
00496 
00497                         const std::string sOdomName = vehVarName("odom", veh);
00498                         const std::string sBaseLinkFrame = vehVarName("base_link", veh);
00499 
00500                         // 1) Ground-truth pose and velocity
00501                         // --------------------------------------------
00502                         const mrpt::math::TPose3D& gh_veh_pose = veh->getPose();
00503                         const mvsim::vec3& gh_veh_vel =
00504                                 veh->getVelocity();  // [vx,vy,w] in global frame
00505 
00506                         {
00507                                 nav_msgs::Odometry gtOdoMsg;
00508 
00509                                 gtOdoMsg.pose.pose.position.x = gh_veh_pose.x;
00510                                 gtOdoMsg.pose.pose.position.y = gh_veh_pose.y;
00511                                 gtOdoMsg.pose.pose.position.z = gh_veh_pose.z;
00512 
00513                                 tf::Quaternion quat;
00514                                 quat.setEuler(
00515                                         gh_veh_pose.roll, gh_veh_pose.pitch, gh_veh_pose.yaw);
00516 
00517                                 gtOdoMsg.pose.pose.orientation.x = quat.x();
00518                                 gtOdoMsg.pose.pose.orientation.y = quat.y();
00519                                 gtOdoMsg.pose.pose.orientation.z = quat.z();
00520                                 gtOdoMsg.pose.pose.orientation.w = quat.w();
00521                                 gtOdoMsg.twist.twist.linear.x = gh_veh_vel.vals[0];
00522                                 gtOdoMsg.twist.twist.linear.y = gh_veh_vel.vals[1];
00523                                 gtOdoMsg.twist.twist.linear.z = 0;
00524                                 gtOdoMsg.twist.twist.angular.z = gh_veh_vel.vals[2];
00525 
00526                                 gtOdoMsg.header.stamp = m_sim_time;
00527                                 gtOdoMsg.header.frame_id = sOdomName;
00528                                 gtOdoMsg.child_frame_id = sBaseLinkFrame;
00529 
00530                                 m_pubsub_vehicles[i].pub_ground_truth.publish(gtOdoMsg);
00531 
00532                                 if (m_do_fake_localization)
00533                                 {
00534                                         geometry_msgs::PoseWithCovarianceStamped currentPos;
00535                                         geometry_msgs::PoseArray particleCloud;
00536 
00537                                         // topic: <Ri>/particlecloud
00538                                         if (m_pubsub_vehicles[i]
00539                                                         .pub_particlecloud.getNumSubscribers() > 0)
00540                                         {
00541                                                 particleCloud.header.stamp = m_sim_time;
00542                                                 particleCloud.header.frame_id = "/map";
00543                                                 particleCloud.poses.resize(1);
00544                                                 particleCloud.poses[0] = gtOdoMsg.pose.pose;
00545                                                 m_pubsub_vehicles[i].pub_particlecloud.publish(
00546                                                         particleCloud);
00547                                         }
00548 
00549                                         // topic: <Ri>/amcl_pose
00550                                         if (m_pubsub_vehicles[i].pub_amcl_pose.getNumSubscribers() >
00551                                                 0)
00552                                         {
00553                                                 currentPos.header = gtOdoMsg.header;
00554                                                 currentPos.pose.pose = gtOdoMsg.pose.pose;
00555                                                 m_pubsub_vehicles[i].pub_amcl_pose.publish(currentPos);
00556                                         }
00557 
00558                                         // TF: /map -> <Ri>/odom
00559                                         {
00560                                                 MRPT_TODO(
00561                                                         "Save initial pose for each vehicle, set odometry "
00562                                                         "from that pose");
00563                                                 const tf::Transform tr(
00564                                                         tf::createIdentityQuaternion(),
00565                                                         tf::Vector3(0, 0, 0));
00566                                                 m_tf_br.sendTransform(
00567                                                         tf::StampedTransform(
00568                                                                 tr, m_sim_time, "/map", sOdomName));
00569                                         }
00570                                 }
00571                         }
00572 
00573                         // 2) Chassis markers (for rviz visualization)
00574                         // --------------------------------------------
00575                         // pub: <VEH>/chassis_markers
00576                         if (m_pubsub_vehicles[i].pub_chassis_markers.getNumSubscribers() >
00577                                 0)
00578                         {
00579                                 visualization_msgs::MarkerArray& msg_shapes =
00580                                         m_pubsub_vehicles[i].chassis_shape_msg;
00581                                 ROS_ASSERT(
00582                                         msg_shapes.markers.size() == (1 + veh->getNumWheels()));
00583 
00584                                 // [0] Chassis shape: static no need to update.
00585                                 // [1:N] Wheel shapes: may move
00586                                 for (size_t j = 0; j < veh->getNumWheels(); j++)
00587                                 {
00588                                         visualization_msgs::Marker& wheel_shape_msg =
00589                                                 msg_shapes.markers[1 + j];
00590                                         const mvsim::Wheel& w = veh->getWheelInfo(j);
00591 
00592                                         // Set local pose of the wheel wrt the vehicle:
00593                                         tf::Matrix3x3 rot;
00594                                         rot.setEulerYPR(w.yaw, 0, 0);
00595                                         tf::poseTFToMsg(
00596                                                 tf::Transform(rot, tf::Vector3(w.x, w.y, 0)),
00597                                                 wheel_shape_msg.pose);
00598                                 }  // end for each wheel
00599 
00600                                 // Publish Initial pose
00601                                 m_pubsub_vehicles[i].pub_chassis_markers.publish(msg_shapes);
00602                         }
00603 
00604                         // 3) odometry transform
00605                         // --------------------------------------------
00606                         {
00607                                 const mrpt::math::TPose3D odo_pose = gh_veh_pose;
00608 
00609                                 {
00610                                         tf::Matrix3x3 rot;
00611                                         rot.setEulerYPR(
00612                                                 odo_pose.yaw, odo_pose.pitch, odo_pose.roll);
00613                                         const tf::Transform tr(
00614                                                 rot, tf::Vector3(odo_pose.x, odo_pose.y, odo_pose.z));
00615 
00616                                         m_tf_br.sendTransform(
00617                                                 tf::StampedTransform(
00618                                                         tr, m_sim_time, sOdomName, sBaseLinkFrame));
00619                                 }
00620 
00621                                 // Apart from TF, publish to the "odom" topic as well
00622                                 if (m_pubsub_vehicles[i].pub_odom.getNumSubscribers() > 0)
00623                                 {
00624                                         nav_msgs::Odometry odoMsg;
00625 
00626                                         odoMsg.pose.pose.position.x = odo_pose.x;
00627                                         odoMsg.pose.pose.position.y = odo_pose.y;
00628                                         odoMsg.pose.pose.position.z = odo_pose.z;
00629 
00630                                         tf::Quaternion quat;
00631                                         quat.setEuler(odo_pose.roll, odo_pose.pitch, odo_pose.yaw);
00632 
00633                                         odoMsg.pose.pose.orientation.x = quat.x();
00634                                         odoMsg.pose.pose.orientation.y = quat.y();
00635                                         odoMsg.pose.pose.orientation.z = quat.z();
00636                                         odoMsg.pose.pose.orientation.w = quat.w();
00637 
00638                                         // first, we'll populate the header for the odometry msg
00639                                         odoMsg.header.stamp = m_sim_time;
00640                                         odoMsg.header.frame_id = sOdomName;
00641                                         odoMsg.child_frame_id = sBaseLinkFrame;
00642 
00643                                         // publish:
00644                                         m_pubsub_vehicles[i].pub_odom.publish(odoMsg);
00645                                 }
00646                         }
00647 
00648                 }  // end for each vehicle
00649 
00650         }  // end publish tf
00651 
00652 }  // end spinNotifyROS()
00653 
00654 void MVSimNode::MyWorld::onNewObservation(
00655         const mvsim::VehicleBase& veh,
00656 #if MRPT_VERSION >= 0x130
00657         const mrpt::obs::CObservation* obs
00658 #else
00659         const mrpt::slam::CObservation* obs
00660 #endif
00661         )
00662 {
00663         ROS_ASSERT(obs);
00664         ROS_ASSERT(!obs->sensorLabel.empty());
00665 
00666         TPubSubPerVehicle& pubs = m_parent.m_pubsub_vehicles[veh.getVehicleIndex()];
00667 
00668         // Create the publisher the first time an observation arrives:
00669         const bool is_1st_pub =
00670                 pubs.pub_sensors.find(obs->sensorLabel) == pubs.pub_sensors.end();
00671         ros::Publisher& pub = pubs.pub_sensors[obs->sensorLabel];
00672 
00673         // Observation: 2d laser scans
00674         // -----------------------------
00675         if (dynamic_cast<const CObservation2DRangeScan*>(obs))
00676         {
00677                 if (is_1st_pub)
00678                         pub = m_parent.m_n.advertise<sensor_msgs::LaserScan>(
00679                                 m_parent.vehVarName(obs->sensorLabel, &veh), 10);
00680 
00681                 const CObservation2DRangeScan* o =
00682                         dynamic_cast<const CObservation2DRangeScan*>(obs);
00683                 const std::string sSensorFrameId =
00684                         m_parent.vehVarName(obs->sensorLabel, &veh);
00685 
00686                 // Send TF:
00687                 mrpt::poses::CPose3D pose_laser;
00688                 tf::Transform transform;
00689                 o->getSensorPose(pose_laser);
00690                 mrpt_bridge::convert(pose_laser, transform);
00691 
00692                 m_parent.m_tf_br.sendTransform(
00693                         tf::StampedTransform(
00694                                 transform, m_parent.m_sim_time,
00695                                 m_parent.vehVarName("base_link", &veh),  // parent frame
00696                                 sSensorFrameId));
00697 
00698                 // Send observation:
00699                 if (is_1st_pub || pub.getNumSubscribers() > 0)
00700                 {
00701                         // Convert observation MRPT -> ROS
00702                         geometry_msgs::Pose msg_pose_laser;
00703                         sensor_msgs::LaserScan msg_laser;
00704                         mrpt_bridge::convert(*o, msg_laser, msg_pose_laser);
00705 
00706                         // Force usage of simulation time:
00707                         msg_laser.header.stamp = m_parent.m_sim_time;
00708                         msg_laser.header.frame_id = sSensorFrameId;
00709 
00710                         pub.publish(msg_laser);
00711                 }
00712         }
00713         else
00714         {
00715                 // Don't know how to emit this observation to ROS!
00716         }
00717 
00718 }  // end of onNewObservation()
00719 
00722 std::string MVSimNode::vehVarName(
00723         const std::string& sVarName, const mvsim::VehicleBase* veh) const
00724 {
00725         if (mvsim_world_.getListOfVehicles().size() == 1)
00726         {
00727                 return std::string("/") + sVarName;
00728         }
00729         else
00730         {
00731                 return std::string("/") + veh->getName() + std::string("/") + sVarName;
00732         }
00733 }


mvsim
Author(s):
autogenerated on Thu Sep 7 2017 09:27:48