From cea38a462dce98baa87c0b813de920226ad8d014 Mon Sep 17 00:00:00 2001 From: Zach Ghera Date: Wed, 30 Dec 2020 20:25:12 -0500 Subject: [PATCH] Added the stage_ros package to our catkin ws for modification. --- src/stage_ros_mod_tf/.travis.yml | 39 + src/stage_ros_mod_tf/CHANGELOG.rst | 57 ++ src/stage_ros_mod_tf/CMakeLists.txt | 66 ++ src/stage_ros_mod_tf/package.xml | 39 + src/stage_ros_mod_tf/rviz/stage.rviz | 157 ++++ src/stage_ros_mod_tf/scripts/upgrade-world.sh | 10 + src/stage_ros_mod_tf/src/stageros.cpp | 785 ++++++++++++++++++ src/stage_ros_mod_tf/test/hztest.xml | 44 + src/stage_ros_mod_tf/test/intensity_test.py | 56 ++ src/stage_ros_mod_tf/test/intensity_test.xml | 8 + src/stage_ros_mod_tf/world/erratic-inc.world | 26 + src/stage_ros_mod_tf/world/intense.world | 63 ++ .../world/willow-erratic.world | 83 ++ .../willow-four-erratics-multisensor.world | 87 ++ .../world/willow-four-erratics.world | 75 ++ src/stage_ros_mod_tf/world/willow-full.pgm | 11 + 16 files changed, 1606 insertions(+) create mode 100644 src/stage_ros_mod_tf/.travis.yml create mode 100644 src/stage_ros_mod_tf/CHANGELOG.rst create mode 100644 src/stage_ros_mod_tf/CMakeLists.txt create mode 100644 src/stage_ros_mod_tf/package.xml create mode 100644 src/stage_ros_mod_tf/rviz/stage.rviz create mode 100755 src/stage_ros_mod_tf/scripts/upgrade-world.sh create mode 100644 src/stage_ros_mod_tf/src/stageros.cpp create mode 100644 src/stage_ros_mod_tf/test/hztest.xml create mode 100755 src/stage_ros_mod_tf/test/intensity_test.py create mode 100644 src/stage_ros_mod_tf/test/intensity_test.xml create mode 100644 src/stage_ros_mod_tf/world/erratic-inc.world create mode 100644 src/stage_ros_mod_tf/world/intense.world create mode 100644 src/stage_ros_mod_tf/world/willow-erratic.world create mode 100644 src/stage_ros_mod_tf/world/willow-four-erratics-multisensor.world create mode 100644 src/stage_ros_mod_tf/world/willow-four-erratics.world create mode 100644 src/stage_ros_mod_tf/world/willow-full.pgm diff --git a/src/stage_ros_mod_tf/.travis.yml b/src/stage_ros_mod_tf/.travis.yml new file mode 100644 index 0000000..e66c0e2 --- /dev/null +++ b/src/stage_ros_mod_tf/.travis.yml @@ -0,0 +1,39 @@ + +sudo: required +dist: trusty +# Force travis to use its minimal image with default Python settings +language: generic +compiler: + - gcc +env: + global: + - CATKIN_WS=~/catkin_ws + - CATKIN_WS_SRC=${CATKIN_WS}/src + matrix: + - CI_ROS_DISTRO="indigo" + - CI_ROS_DISTRO="jade" +install: + - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list' + - wget http://packages.ros.org/ros.key -O - | sudo apt-key add - + - sudo apt-get update -qq + - sudo apt-get install -qq -y python-rosdep python-catkin-tools + - sudo rosdep init + - rosdep update + # Use rosdep to install all dependencies (including ROS itself) + - rosdep install --from-paths ./ -i -y --rosdistro $CI_ROS_DISTRO +script: + - source /opt/ros/$CI_ROS_DISTRO/setup.bash + - mkdir -p $CATKIN_WS_SRC + - ln -s $TRAVIS_BUILD_DIR $CATKIN_WS_SRC + - cd $CATKIN_WS + - catkin init + # Enable install space + - catkin config --install + # Build [and Install] packages + - catkin build --no-status -vi --no-notify -DCMAKE_BUILD_TYPE=Release + # Build tests + - catkin build --no-status -vi --no-notify --make-args tests + # Run tests + - catkin run_tests + # Assert tests all passed + - catkin_test_results ./build diff --git a/src/stage_ros_mod_tf/CHANGELOG.rst b/src/stage_ros_mod_tf/CHANGELOG.rst new file mode 100644 index 0000000..4b8b351 --- /dev/null +++ b/src/stage_ros_mod_tf/CHANGELOG.rst @@ -0,0 +1,57 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package stage_ros +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.8.0 (2017-04-30) +------------------ +* Now uses Stage's native event loop properly and added reassuring startup output. +* Added a GUI section so that the world starts in a good place. +* Fixed issue such that ranger intensity values are no longer clipped to 256 + See: `#31 `_ +* Contributors: Richard Vaughan, Shane Loretz, William Woodall, gerkey + +1.7.5 (2015-09-16) +------------------ +* Removed all references to FLTK/Fluid and use the upstream CMake config file instead. +* Added ``reset_positions`` service to stage (adds dependency on ``std_srvs``). +* Contributors: Aurélien Ballier, Daniel Claes, Scott K Logan, William Woodall + +1.7.4 (2015-03-04) +------------------ +* Added missing -ldl flag on newer versions of Ubuntu +* Contributors: William Woodall + +1.7.3 (2015-01-26) +------------------ +* Split up ``fltk`` dep into ``libfltk-dev`` and ``fluid``, only ``run_depend``'ing on fluid. +* Now supports multiple robots with multiple sensors. +* Fixed a bug on systems that cannot populate FLTK_INCLUDE_DIRS. +* Updated topurg model from "laser" to "ranger". +* Added -u option to use name property of position models as its namespace instead of "robot_0", "robot_1", etc. +* Contributors: Gustavo Velasco Hernández, Gustavo Velasco-Hernández, Pablo Urcola, Wayne Chang, William Woodall + +1.7.2 (2013-09-19) +------------------ +* Changed default GUI window size to 600x400 +* Added velocity to ground truth odometry +* Fixed tf (yaw component) for the base_link->camera transform. +* Fixed ground truth pose coordinate system + +1.7.1 (2013-08-30) +------------------ +* Fixing warnings +* Small fixes +* Added RGB+3D-sensor interface (Primesense/Kinect/Xtion). + * Publishes CameraInfo, depth image, RGBA image, tf (takes world-file pantilt paremeter into account) + * Supports the "old" configuration (laser+odom) as well as camera+odom, laser+camera+odom and odom-only. + Fixed laser transform height (previously was hardcoded at 0.15, now it takes robot height into account). +* Introduced changes from https://github.com/rtv/Stage/issues/34 with some changes (does not require lasers to be present and works without cameras). + +1.7.0 (2013-06-27 18:15:07 -0700) +--------------------------------- +- Initial move over from old repository: https://code.ros.org/svn/ros-pkg/stacks/stage +- Catkinized +- Stage itself is released as a third party package now +- Had to disable velocities in the output odometry as Stage no longer implements it internally. +- Updated rostest +- Updated rviz configurations diff --git a/src/stage_ros_mod_tf/CMakeLists.txt b/src/stage_ros_mod_tf/CMakeLists.txt new file mode 100644 index 0000000..d6bc35e --- /dev/null +++ b/src/stage_ros_mod_tf/CMakeLists.txt @@ -0,0 +1,66 @@ +cmake_minimum_required(VERSION 2.8.3) +project(stage_ros) + +find_package(catkin REQUIRED + COMPONENTS + geometry_msgs + nav_msgs + roscpp + sensor_msgs + std_msgs + std_srvs + tf +) + +find_package(Boost REQUIRED COMPONENTS system thread) + +find_package(stage REQUIRED) + +include_directories( + ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} + ${STAGE_INCLUDE_DIRS} +) + +catkin_package() + +# Declare a cpp executable +add_executable(stageros src/stageros.cpp) +set(${PROJECT_NAME}_extra_libs "") +if(UNIX AND NOT APPLE) + set(${PROJECT_NAME}_extra_libs dl) +endif() +target_link_libraries(stageros + ${catkin_LIBRARIES} + ${Boost_LIBRARIES} + ${STAGE_LIBRARIES} + ${${PROJECT_NAME}_extra_libs} +) +if(catkin_EXPORTED_TARGETS) + add_dependencies(stageros ${catkin_EXPORTED_TARGETS}) +endif() + +## Install + +install(PROGRAMS scripts/upgrade-world.sh + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + +install(TARGETS stageros + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY rviz + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +install(DIRECTORY world + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +## Tests + +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + add_rostest(test/hztest.xml) + add_rostest(test/intensity_test.xml) +endif() diff --git a/src/stage_ros_mod_tf/package.xml b/src/stage_ros_mod_tf/package.xml new file mode 100644 index 0000000..ef3ccdf --- /dev/null +++ b/src/stage_ros_mod_tf/package.xml @@ -0,0 +1,39 @@ + + + stage_ros + 1.8.0 + This package provides ROS specific hooks for stage + + William Woodall + + BSD + + http://ros.org/wiki/stage_ros + https://github.com/ros-simulation/stage_ros + https://github.com/ros-simulation/stage_ros/issues + + Brian Gerky + + catkin + + boost + geometry_msgs + nav_msgs + roscpp + rostest + sensor_msgs + stage + std_msgs + std_srvs + tf + + boost + geometry_msgs + nav_msgs + roscpp + sensor_msgs + stage + std_msgs + std_srvs + tf + diff --git a/src/stage_ros_mod_tf/rviz/stage.rviz b/src/stage_ros_mod_tf/rviz/stage.rviz new file mode 100644 index 0000000..bc6c5f6 --- /dev/null +++ b/src/stage_ros_mod_tf/rviz/stage.rviz @@ -0,0 +1,157 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /TF1/Frames1 + Splitter Ratio: 0.5 + Tree Height: 479 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: LaserScan +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + /base_footprint: + Value: true + /base_laser_link: + Value: true + /base_link: + Value: true + /odom: + Value: true + All Enabled: false + Marker Scale: 1 + Name: TF + Show Arrows: false + Show Axes: true + Show Names: false + Tree: + /odom: + /base_footprint: + /base_link: + /base_laser_link: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Max Color: 255; 255; 255 + Max Intensity: 1 + Min Color: 0; 0; 0 + Min Intensity: 1 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Flat Squares + Topic: /base_scan + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: /odom + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 5.68654 + Focal Point: + X: 0 + Y: 0 + Z: 0 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.529796 + Target Frame: + Value: Orbit (rviz) + Yaw: 3.2654 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 756 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000013c0000026dfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005600fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000002540000011a00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002c0000026d000000dc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000026dfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002c0000026d000000a500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004fc0000003efc0100000002fb0000000800540069006d00650100000000000004fc0000020b00fffffffb0000000800540069006d00650100000000000004500000000000000000000002af0000026d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1276 + X: 60 + Y: 22 diff --git a/src/stage_ros_mod_tf/scripts/upgrade-world.sh b/src/stage_ros_mod_tf/scripts/upgrade-world.sh new file mode 100755 index 0000000..47ddc86 --- /dev/null +++ b/src/stage_ros_mod_tf/scripts/upgrade-world.sh @@ -0,0 +1,10 @@ +#!/bin/bash + +USAGE="USAGE: upgrade-world.sh in.world out.world" + +if [ $# != 2 ]; then + echo $USAGE + exit 1 +fi + +cat < $1 | sed 's/size3/size/' | sed 's/origin3/origin/' | sed 's/.*interval_real.*//' | sed 's/.*range_min.*//' | sed 's/.*center.*//' | sed 's/.*gui_movemask.*//' | sed 's/\(.*pose *\[\)\([^ ]*\) *\([^ ]*\) *\([^ ]*\) *\(\].*\)/\1 \2 \3 0 \4 \5/' > $2 diff --git a/src/stage_ros_mod_tf/src/stageros.cpp b/src/stage_ros_mod_tf/src/stageros.cpp new file mode 100644 index 0000000..39d040a --- /dev/null +++ b/src/stage_ros_mod_tf/src/stageros.cpp @@ -0,0 +1,785 @@ +/* + * stageros + * Copyright (c) 2008, Willow Garage, Inc. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +/** + +@mainpage + +@htmlinclude manifest.html +**/ + +#include +#include +#include + +#include +#include +#include +#include + + +// libstage +#include + + +// roscpp +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "tf/transform_broadcaster.h" + +#define USAGE "stageros " +#define IMAGE "image" +#define DEPTH "depth" +#define CAMERA_INFO "camera_info" +#define ODOM "odom" +#define BASE_SCAN "base_scan" +#define BASE_POSE_GROUND_TRUTH "base_pose_ground_truth" +#define CMD_VEL "cmd_vel" + +// Our node +class StageNode +{ +private: + + // roscpp-related bookkeeping + ros::NodeHandle n_; + + // A mutex to lock access to fields that are used in message callbacks + boost::mutex msg_lock; + + // The models that we're interested in + std::vector cameramodels; + std::vector lasermodels; + std::vector positionmodels; + + //a structure representing a robot inthe simulator + struct StageRobot + { + //stage related models + Stg::ModelPosition* positionmodel; //one position + std::vector cameramodels; //multiple cameras per position + std::vector lasermodels; //multiple rangers per position + + //ros publishers + ros::Publisher odom_pub; //one odom + ros::Publisher ground_truth_pub; //one ground truth + + std::vector image_pubs; //multiple images + std::vector depth_pubs; //multiple depths + std::vector camera_pubs; //multiple cameras + std::vector laser_pubs; //multiple lasers + + ros::Subscriber cmdvel_sub; //one cmd_vel subscriber + }; + + std::vector robotmodels_; + + // Used to remember initial poses for soft reset + std::vector initial_poses; + ros::ServiceServer reset_srv_; + + ros::Publisher clock_pub_; + + bool isDepthCanonical; + bool use_model_names; + + // A helper function that is executed for each stage model. We use it + // to search for models of interest. + static void ghfunc(Stg::Model* mod, StageNode* node); + + static bool s_update(Stg::World* world, StageNode* node) + { + node->WorldCallback(); + // We return false to indicate that we want to be called again (an + // odd convention, but that's the way that Stage works). + return false; + } + + // Appends the given robot ID to the given message name. If omitRobotID + // is true, an unaltered copy of the name is returned. + const char *mapName(const char *name, size_t robotID, Stg::Model* mod) const; + const char *mapName(const char *name, size_t robotID, size_t deviceID, Stg::Model* mod) const; + + tf::TransformBroadcaster tf; + + // Last time that we received a velocity command + ros::Time base_last_cmd; + ros::Duration base_watchdog_timeout; + + // Current simulation time + ros::Time sim_time; + + // Last time we saved global position (for velocity calculation). + ros::Time base_last_globalpos_time; + // Last published global pose of each robot + std::vector base_last_globalpos; + +public: + // Constructor; stage itself needs argc/argv. fname is the .world file + // that stage should load. + StageNode(int argc, char** argv, bool gui, const char* fname, bool use_model_names); + ~StageNode(); + + // Subscribe to models of interest. Currently, we find and subscribe + // to the first 'laser' model and the first 'position' model. Returns + // 0 on success (both models subscribed), -1 otherwise. + int SubscribeModels(); + + // Our callback + void WorldCallback(); + + // Do one update of the world. May pause if the next update time + // has not yet arrived. + bool UpdateWorld(); + + // Message callback for a MsgBaseVel message, which set velocities. + void cmdvelReceived(int idx, const boost::shared_ptr& msg); + + // Service callback for soft reset + bool cb_reset_srv(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response); + + // The main simulator object + Stg::World* world; +}; + +// since stageros is single-threaded, this is OK. revisit if that changes! +const char * +StageNode::mapName(const char *name, size_t robotID, Stg::Model* mod) const +{ + //ROS_INFO("Robot %lu: Device %s", robotID, name); + bool umn = this->use_model_names; + + if ((positionmodels.size() > 1 ) || umn) + { + static char buf[100]; + std::size_t found = std::string(((Stg::Ancestor *) mod)->Token()).find(":"); + + if ((found==std::string::npos) && umn) + { + snprintf(buf, sizeof(buf), "/%s/%s", ((Stg::Ancestor *) mod)->Token(), name); + } + else + { + snprintf(buf, sizeof(buf), "/robot_%u/%s", (unsigned int)robotID, name); + } + + return buf; + } + else + return name; +} + +const char * +StageNode::mapName(const char *name, size_t robotID, size_t deviceID, Stg::Model* mod) const +{ + //ROS_INFO("Robot %lu: Device %s:%lu", robotID, name, deviceID); + bool umn = this->use_model_names; + + if ((positionmodels.size() > 1 ) || umn) + { + static char buf[100]; + std::size_t found = std::string(((Stg::Ancestor *) mod)->Token()).find(":"); + + if ((found==std::string::npos) && umn) + { + snprintf(buf, sizeof(buf), "/%s/%s_%u", ((Stg::Ancestor *) mod)->Token(), name, (unsigned int)deviceID); + } + else + { + snprintf(buf, sizeof(buf), "/robot_%u/%s_%u", (unsigned int)robotID, name, (unsigned int)deviceID); + } + + return buf; + } + else + { + static char buf[100]; + snprintf(buf, sizeof(buf), "/%s_%u", name, (unsigned int)deviceID); + return buf; + } +} + +void +StageNode::ghfunc(Stg::Model* mod, StageNode* node) +{ + //printf( "inspecting %s, parent\n", mod->Token() ); + + if (dynamic_cast(mod)) { + node->lasermodels.push_back(dynamic_cast(mod)); + } + if (dynamic_cast(mod)) { + Stg::ModelPosition * p = dynamic_cast(mod); + // remember initial poses + node->positionmodels.push_back(p); + node->initial_poses.push_back(p->GetGlobalPose()); + } + if (dynamic_cast(mod)) { + node->cameramodels.push_back(dynamic_cast(mod)); + } +} + + + + +bool +StageNode::cb_reset_srv(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) +{ + ROS_INFO("Resetting stage!"); + for (size_t r = 0; r < this->positionmodels.size(); r++) { + this->positionmodels[r]->SetPose(this->initial_poses[r]); + this->positionmodels[r]->SetStall(false); + } + return true; +} + + + +void +StageNode::cmdvelReceived(int idx, const boost::shared_ptr& msg) +{ + boost::mutex::scoped_lock lock(msg_lock); + this->positionmodels[idx]->SetSpeed(msg->linear.x, + msg->linear.y, + msg->angular.z); + this->base_last_cmd = this->sim_time; +} + +StageNode::StageNode(int argc, char** argv, bool gui, const char* fname, bool use_model_names) +{ + this->use_model_names = use_model_names; + this->sim_time.fromSec(0.0); + this->base_last_cmd.fromSec(0.0); + double t; + ros::NodeHandle localn("~"); + if(!localn.getParam("base_watchdog_timeout", t)) + t = 0.2; + this->base_watchdog_timeout.fromSec(t); + + if(!localn.getParam("is_depth_canonical", isDepthCanonical)) + isDepthCanonical = true; + + // We'll check the existence of the world file, because libstage doesn't + // expose its failure to open it. Could go further with checks (e.g., is + // it readable by this user). + struct stat s; + if(stat(fname, &s) != 0) + { + ROS_FATAL("The world file %s does not exist.", fname); + ROS_BREAK(); + } + + // initialize libstage + Stg::Init( &argc, &argv ); + + if(gui) + this->world = new Stg::WorldGui(600, 400, "Stage (ROS)"); + else + this->world = new Stg::World(); + + this->world->Load(fname); + + // todo: reverse the order of these next lines? try it . + + this->world->AddUpdateCallback((Stg::world_callback_t)s_update, this); + + // inspect every model to locate the things we care about + this->world->ForEachDescendant((Stg::model_callback_t)ghfunc, this); +} + + +// Subscribe to models of interest. Currently, we find and subscribe +// to the first 'laser' model and the first 'position' model. Returns +// 0 on success (both models subscribed), -1 otherwise. +// +// Eventually, we should provide a general way to map stage models onto ROS +// topics, similar to Player .cfg files. +int +StageNode::SubscribeModels() +{ + n_.setParam("/use_sim_time", true); + + for (size_t r = 0; r < this->positionmodels.size(); r++) + { + StageRobot* new_robot = new StageRobot; + new_robot->positionmodel = this->positionmodels[r]; + new_robot->positionmodel->Subscribe(); + + ROS_INFO( "Subscribed to Stage position model \"%s\"", this->positionmodels[r]->Token() ); + + for (size_t s = 0; s < this->lasermodels.size(); s++) + { + if (this->lasermodels[s] and this->lasermodels[s]->Parent() == new_robot->positionmodel) + { + new_robot->lasermodels.push_back(this->lasermodels[s]); + this->lasermodels[s]->Subscribe(); + ROS_INFO( "subscribed to Stage ranger \"%s\"", this->lasermodels[s]->Token() ); + } + } + + for (size_t s = 0; s < this->cameramodels.size(); s++) + { + if (this->cameramodels[s] and this->cameramodels[s]->Parent() == new_robot->positionmodel) + { + new_robot->cameramodels.push_back(this->cameramodels[s]); + this->cameramodels[s]->Subscribe(); + + ROS_INFO( "subscribed to Stage camera model \"%s\"", this->cameramodels[s]->Token() ); + } + } + + // TODO - print the topic names nicely as well + ROS_INFO("Robot %s provided %lu rangers and %lu cameras", + new_robot->positionmodel->Token(), + new_robot->lasermodels.size(), + new_robot->cameramodels.size() ); + + new_robot->odom_pub = n_.advertise(mapName(ODOM, r, static_cast(new_robot->positionmodel)), 10); + new_robot->ground_truth_pub = n_.advertise(mapName(BASE_POSE_GROUND_TRUTH, r, static_cast(new_robot->positionmodel)), 10); + new_robot->cmdvel_sub = n_.subscribe(mapName(CMD_VEL, r, static_cast(new_robot->positionmodel)), 10, boost::bind(&StageNode::cmdvelReceived, this, r, _1)); + + for (size_t s = 0; s < new_robot->lasermodels.size(); ++s) + { + if (new_robot->lasermodels.size() == 1) + new_robot->laser_pubs.push_back(n_.advertise(mapName(BASE_SCAN, r, static_cast(new_robot->positionmodel)), 10)); + else + new_robot->laser_pubs.push_back(n_.advertise(mapName(BASE_SCAN, r, s, static_cast(new_robot->positionmodel)), 10)); + + } + + for (size_t s = 0; s < new_robot->cameramodels.size(); ++s) + { + if (new_robot->cameramodels.size() == 1) + { + new_robot->image_pubs.push_back(n_.advertise(mapName(IMAGE, r, static_cast(new_robot->positionmodel)), 10)); + new_robot->depth_pubs.push_back(n_.advertise(mapName(DEPTH, r, static_cast(new_robot->positionmodel)), 10)); + new_robot->camera_pubs.push_back(n_.advertise(mapName(CAMERA_INFO, r, static_cast(new_robot->positionmodel)), 10)); + } + else + { + new_robot->image_pubs.push_back(n_.advertise(mapName(IMAGE, r, s, static_cast(new_robot->positionmodel)), 10)); + new_robot->depth_pubs.push_back(n_.advertise(mapName(DEPTH, r, s, static_cast(new_robot->positionmodel)), 10)); + new_robot->camera_pubs.push_back(n_.advertise(mapName(CAMERA_INFO, r, s, static_cast(new_robot->positionmodel)), 10)); + } + } + + this->robotmodels_.push_back(new_robot); + } + clock_pub_ = n_.advertise("/clock", 10); + + // advertising reset service + reset_srv_ = n_.advertiseService("reset_positions", &StageNode::cb_reset_srv, this); + + return(0); +} + +StageNode::~StageNode() +{ + for (std::vector::iterator r = this->robotmodels_.begin(); r != this->robotmodels_.end(); ++r) + delete *r; +} + +bool +StageNode::UpdateWorld() +{ + return this->world->UpdateAll(); +} + +void +StageNode::WorldCallback() +{ + if( ! ros::ok() ) { + ROS_INFO( "ros::ok() is false. Quitting." ); + this->world->QuitAll(); + return; + } + + boost::mutex::scoped_lock lock(msg_lock); + + this->sim_time.fromSec(world->SimTimeNow() / 1e6); + // We're not allowed to publish clock==0, because it used as a special + // value in parts of ROS, #4027. + if(this->sim_time.sec == 0 && this->sim_time.nsec == 0) + { + ROS_DEBUG("Skipping initial simulation step, to avoid publishing clock==0"); + return; + } + + // TODO make this only affect one robot if necessary + if((this->base_watchdog_timeout.toSec() > 0.0) && + ((this->sim_time - this->base_last_cmd) >= this->base_watchdog_timeout)) + { + for (size_t r = 0; r < this->positionmodels.size(); r++) + this->positionmodels[r]->SetSpeed(0.0, 0.0, 0.0); + } + + //loop on the robot models + for (size_t r = 0; r < this->robotmodels_.size(); ++r) + { + StageRobot const * robotmodel = this->robotmodels_[r]; + + //loop on the laser devices for the current robot + for (size_t s = 0; s < robotmodel->lasermodels.size(); ++s) + { + Stg::ModelRanger const* lasermodel = robotmodel->lasermodels[s]; + const std::vector& sensors = lasermodel->GetSensors(); + + if( sensors.size() > 1 ) + ROS_WARN( "ROS Stage currently supports rangers with 1 sensor only." ); + + // for now we access only the zeroth sensor of the ranger - good + // enough for most laser models that have a single beam origin + const Stg::ModelRanger::Sensor& sensor = sensors[0]; + + if( sensor.ranges.size() ) + { + // Translate into ROS message format and publish + sensor_msgs::LaserScan msg; + msg.angle_min = -sensor.fov/2.0; + msg.angle_max = +sensor.fov/2.0; + msg.angle_increment = sensor.fov/(double)(sensor.sample_count-1); + msg.range_min = sensor.range.min; + msg.range_max = sensor.range.max; + msg.ranges.resize(sensor.ranges.size()); + msg.intensities.resize(sensor.intensities.size()); + + for(unsigned int i = 0; i < sensor.ranges.size(); i++) + { + msg.ranges[i] = sensor.ranges[i]; + msg.intensities[i] = sensor.intensities[i]; + } + + if (robotmodel->lasermodels.size() > 1) + msg.header.frame_id = mapName("base_laser_link", r, s, static_cast(robotmodel->positionmodel)); + else + msg.header.frame_id = mapName("base_laser_link", r, static_cast(robotmodel->positionmodel)); + + msg.header.stamp = sim_time; + robotmodel->laser_pubs[s].publish(msg); + } + + // Also publish the base->base_laser_link Tx. This could eventually move + // into being retrieved from the param server as a static Tx. + Stg::Pose lp = lasermodel->GetPose(); + tf::Quaternion laserQ; + laserQ.setRPY(0.0, 0.0, lp.a); + tf::Transform txLaser = tf::Transform(laserQ, tf::Point(lp.x, lp.y, robotmodel->positionmodel->GetGeom().size.z + lp.z)); + + if (robotmodel->lasermodels.size() > 1) + tf.sendTransform(tf::StampedTransform(txLaser, sim_time, + mapName("base_link", r, static_cast(robotmodel->positionmodel)), + mapName("base_laser_link", r, s, static_cast(robotmodel->positionmodel)))); + else + tf.sendTransform(tf::StampedTransform(txLaser, sim_time, + mapName("base_link", r, static_cast(robotmodel->positionmodel)), + mapName("base_laser_link", r, static_cast(robotmodel->positionmodel)))); + } + + //the position of the robot + tf.sendTransform(tf::StampedTransform(tf::Transform::getIdentity(), + sim_time, + mapName("base_footprint", r, static_cast(robotmodel->positionmodel)), + mapName("base_link", r, static_cast(robotmodel->positionmodel)))); + + // Get latest odometry data + // Translate into ROS message format and publish + nav_msgs::Odometry odom_msg; + odom_msg.pose.pose.position.x = robotmodel->positionmodel->est_pose.x; + odom_msg.pose.pose.position.y = robotmodel->positionmodel->est_pose.y; + odom_msg.pose.pose.orientation = tf::createQuaternionMsgFromYaw(robotmodel->positionmodel->est_pose.a); + Stg::Velocity v = robotmodel->positionmodel->GetVelocity(); + odom_msg.twist.twist.linear.x = v.x; + odom_msg.twist.twist.linear.y = v.y; + odom_msg.twist.twist.angular.z = v.a; + + //@todo Publish stall on a separate topic when one becomes available + //this->odomMsgs[r].stall = this->positionmodels[r]->Stall(); + // + odom_msg.header.frame_id = mapName("odom", r, static_cast(robotmodel->positionmodel)); + odom_msg.header.stamp = sim_time; + + robotmodel->odom_pub.publish(odom_msg); + + // broadcast odometry transform + tf::Quaternion odomQ; + tf::quaternionMsgToTF(odom_msg.pose.pose.orientation, odomQ); + tf::Transform txOdom(odomQ, tf::Point(odom_msg.pose.pose.position.x, odom_msg.pose.pose.position.y, 0.0)); + tf.sendTransform(tf::StampedTransform(txOdom, sim_time, + mapName("odom", r, static_cast(robotmodel->positionmodel)), + mapName("base_footprint", r, static_cast(robotmodel->positionmodel)))); + + // Also publish the ground truth pose and velocity + Stg::Pose gpose = robotmodel->positionmodel->GetGlobalPose(); + tf::Quaternion q_gpose; + q_gpose.setRPY(0.0, 0.0, gpose.a); + tf::Transform gt(q_gpose, tf::Point(gpose.x, gpose.y, 0.0)); + // Velocity is 0 by default and will be set only if there is previous pose and time delta>0 + Stg::Velocity gvel(0,0,0,0); + if (this->base_last_globalpos.size()>r){ + Stg::Pose prevpose = this->base_last_globalpos.at(r); + double dT = (this->sim_time-this->base_last_globalpos_time).toSec(); + if (dT>0) + gvel = Stg::Velocity( + (gpose.x - prevpose.x)/dT, + (gpose.y - prevpose.y)/dT, + (gpose.z - prevpose.z)/dT, + Stg::normalize(gpose.a - prevpose.a)/dT + ); + this->base_last_globalpos.at(r) = gpose; + }else //There are no previous readings, adding current pose... + this->base_last_globalpos.push_back(gpose); + + nav_msgs::Odometry ground_truth_msg; + ground_truth_msg.pose.pose.position.x = gt.getOrigin().x(); + ground_truth_msg.pose.pose.position.y = gt.getOrigin().y(); + ground_truth_msg.pose.pose.position.z = gt.getOrigin().z(); + ground_truth_msg.pose.pose.orientation.x = gt.getRotation().x(); + ground_truth_msg.pose.pose.orientation.y = gt.getRotation().y(); + ground_truth_msg.pose.pose.orientation.z = gt.getRotation().z(); + ground_truth_msg.pose.pose.orientation.w = gt.getRotation().w(); + ground_truth_msg.twist.twist.linear.x = gvel.x; + ground_truth_msg.twist.twist.linear.y = gvel.y; + ground_truth_msg.twist.twist.linear.z = gvel.z; + ground_truth_msg.twist.twist.angular.z = gvel.a; + + ground_truth_msg.header.frame_id = mapName("odom", r, static_cast(robotmodel->positionmodel)); + ground_truth_msg.header.stamp = sim_time; + + robotmodel->ground_truth_pub.publish(ground_truth_msg); + + //cameras + for (size_t s = 0; s < robotmodel->cameramodels.size(); ++s) + { + Stg::ModelCamera* cameramodel = robotmodel->cameramodels[s]; + // Get latest image data + // Translate into ROS message format and publish + if (robotmodel->image_pubs[s].getNumSubscribers() > 0 && cameramodel->FrameColor()) + { + sensor_msgs::Image image_msg; + + image_msg.height = cameramodel->getHeight(); + image_msg.width = cameramodel->getWidth(); + image_msg.encoding = "rgba8"; + //this->imageMsgs[r].is_bigendian=""; + image_msg.step = image_msg.width*4; + image_msg.data.resize(image_msg.width * image_msg.height * 4); + + memcpy(&(image_msg.data[0]), cameramodel->FrameColor(), image_msg.width * image_msg.height * 4); + + //invert the opengl weirdness + int height = image_msg.height - 1; + int linewidth = image_msg.width*4; + + char* temp = new char[linewidth]; + for (int y = 0; y < (height+1)/2; y++) + { + memcpy(temp,&image_msg.data[y*linewidth],linewidth); + memcpy(&(image_msg.data[y*linewidth]),&(image_msg.data[(height-y)*linewidth]),linewidth); + memcpy(&(image_msg.data[(height-y)*linewidth]),temp,linewidth); + } + + if (robotmodel->cameramodels.size() > 1) + image_msg.header.frame_id = mapName("camera", r, s, static_cast(robotmodel->positionmodel)); + else + image_msg.header.frame_id = mapName("camera", r,static_cast(robotmodel->positionmodel)); + image_msg.header.stamp = sim_time; + + robotmodel->image_pubs[s].publish(image_msg); + } + + //Get latest depth data + //Translate into ROS message format and publish + //Skip if there are no subscribers + if (robotmodel->depth_pubs[s].getNumSubscribers()>0 && cameramodel->FrameDepth()) + { + sensor_msgs::Image depth_msg; + depth_msg.height = cameramodel->getHeight(); + depth_msg.width = cameramodel->getWidth(); + depth_msg.encoding = this->isDepthCanonical?sensor_msgs::image_encodings::TYPE_32FC1:sensor_msgs::image_encodings::TYPE_16UC1; + //this->depthMsgs[r].is_bigendian=""; + int sz = this->isDepthCanonical?sizeof(float):sizeof(uint16_t); + size_t len = depth_msg.width * depth_msg.height; + depth_msg.step = depth_msg.width * sz; + depth_msg.data.resize(len*sz); + + //processing data according to REP118 + if (this->isDepthCanonical){ + double nearClip = cameramodel->getCamera().nearClip(); + double farClip = cameramodel->getCamera().farClip(); + memcpy(&(depth_msg.data[0]),cameramodel->FrameDepth(),len*sz); + float * data = (float*)&(depth_msg.data[0]); + for (size_t i=0;i=farClip) + data[i] = INFINITY; + } + else{ + int nearClip = (int)(cameramodel->getCamera().nearClip() * 1000); + int farClip = (int)(cameramodel->getCamera().farClip() * 1000); + for (size_t i=0;iFrameDepth()[i]*1000); + if (v<=nearClip || v>=farClip) v = 0; + ((uint16_t*)&(depth_msg.data[0]))[i] = (uint16_t) ((v<=nearClip || v>=farClip) ? 0 : v ); + } + } + + //invert the opengl weirdness + int height = depth_msg.height - 1; + int linewidth = depth_msg.width*sz; + + char* temp = new char[linewidth]; + for (int y = 0; y < (height+1)/2; y++) + { + memcpy(temp,&depth_msg.data[y*linewidth],linewidth); + memcpy(&(depth_msg.data[y*linewidth]),&(depth_msg.data[(height-y)*linewidth]),linewidth); + memcpy(&(depth_msg.data[(height-y)*linewidth]),temp,linewidth); + } + + if (robotmodel->cameramodels.size() > 1) + depth_msg.header.frame_id = mapName("camera", r, s, static_cast(robotmodel->positionmodel)); + else + depth_msg.header.frame_id = mapName("camera", r, static_cast(robotmodel->positionmodel)); + depth_msg.header.stamp = sim_time; + robotmodel->depth_pubs[s].publish(depth_msg); + } + + //sending camera's tf and info only if image or depth topics are subscribed to + if ((robotmodel->image_pubs[s].getNumSubscribers()>0 && cameramodel->FrameColor()) + || (robotmodel->depth_pubs[s].getNumSubscribers()>0 && cameramodel->FrameDepth())) + { + + Stg::Pose lp = cameramodel->GetPose(); + tf::Quaternion Q; Q.setRPY( + (cameramodel->getCamera().pitch()*M_PI/180.0)-M_PI, + 0.0, + lp.a+(cameramodel->getCamera().yaw()*M_PI/180.0) - robotmodel->positionmodel->GetPose().a + ); + + tf::Transform tr = tf::Transform(Q, tf::Point(lp.x, lp.y, robotmodel->positionmodel->GetGeom().size.z+lp.z)); + + if (robotmodel->cameramodels.size() > 1) + tf.sendTransform(tf::StampedTransform(tr, sim_time, + mapName("base_link", r, static_cast(robotmodel->positionmodel)), + mapName("camera", r, s, static_cast(robotmodel->positionmodel)))); + else + tf.sendTransform(tf::StampedTransform(tr, sim_time, + mapName("base_link", r, static_cast(robotmodel->positionmodel)), + mapName("camera", r, static_cast(robotmodel->positionmodel)))); + + sensor_msgs::CameraInfo camera_msg; + if (robotmodel->cameramodels.size() > 1) + camera_msg.header.frame_id = mapName("camera", r, s, static_cast(robotmodel->positionmodel)); + else + camera_msg.header.frame_id = mapName("camera", r, static_cast(robotmodel->positionmodel)); + camera_msg.header.stamp = sim_time; + camera_msg.height = cameramodel->getHeight(); + camera_msg.width = cameramodel->getWidth(); + + double fx,fy,cx,cy; + cx = camera_msg.width / 2.0; + cy = camera_msg.height / 2.0; + double fovh = cameramodel->getCamera().horizFov()*M_PI/180.0; + double fovv = cameramodel->getCamera().vertFov()*M_PI/180.0; + //double fx_ = 1.43266615300557*this->cameramodels[r]->getWidth()/tan(fovh); + //double fy_ = 1.43266615300557*this->cameramodels[r]->getHeight()/tan(fovv); + fx = cameramodel->getWidth()/(2*tan(fovh/2)); + fy = cameramodel->getHeight()/(2*tan(fovv/2)); + + //ROS_INFO("fx=%.4f,%.4f; fy=%.4f,%.4f", fx, fx_, fy, fy_); + + + camera_msg.D.resize(4, 0.0); + + camera_msg.K[0] = fx; + camera_msg.K[2] = cx; + camera_msg.K[4] = fy; + camera_msg.K[5] = cy; + camera_msg.K[8] = 1.0; + + camera_msg.R[0] = 1.0; + camera_msg.R[4] = 1.0; + camera_msg.R[8] = 1.0; + + camera_msg.P[0] = fx; + camera_msg.P[2] = cx; + camera_msg.P[5] = fy; + camera_msg.P[6] = cy; + camera_msg.P[10] = 1.0; + + robotmodel->camera_pubs[s].publish(camera_msg); + + } + + } + } + + this->base_last_globalpos_time = this->sim_time; + rosgraph_msgs::Clock clock_msg; + clock_msg.clock = sim_time; + this->clock_pub_.publish(clock_msg); +} + +int +main(int argc, char** argv) +{ + if( argc < 2 ) + { + puts(USAGE); + exit(-1); + } + + ros::init(argc, argv, "stageros"); + + bool gui = true; + bool use_model_names = false; + for(int i=0;i<(argc-1);i++) + { + if(!strcmp(argv[i], "-g")) + gui = false; + if(!strcmp(argv[i], "-u")) + use_model_names = true; + } + + StageNode sn(argc-1,argv,gui,argv[argc-1], use_model_names); + + if(sn.SubscribeModels() != 0) + exit(-1); + + boost::thread t = boost::thread(boost::bind(&ros::spin)); + + sn.world->Start(); + + Stg::World::Run(); + + t.join(); + + exit(0); +} + diff --git a/src/stage_ros_mod_tf/test/hztest.xml b/src/stage_ros_mod_tf/test/hztest.xml new file mode 100644 index 0000000..51c16ab --- /dev/null +++ b/src/stage_ros_mod_tf/test/hztest.xml @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/stage_ros_mod_tf/test/intensity_test.py b/src/stage_ros_mod_tf/test/intensity_test.py new file mode 100755 index 0000000..c70a96d --- /dev/null +++ b/src/stage_ros_mod_tf/test/intensity_test.py @@ -0,0 +1,56 @@ +#!/usr/bin/env python + +import unittest +import rospy +import time +import Queue as queue +from sensor_msgs.msg import LaserScan + +PKG = "stage_ros" + + +class RangerIntensityTests(unittest.TestCase): + def setUp(self): + """Called before every test. Set up a LaserScan subscriber + """ + rospy.init_node('ranger_intensity_test', anonymous=True) + self._scan_q = queue.Queue() + self._scan_topic = rospy.get_param("scan_topic", "base_scan") + self._subscriber = rospy.Subscriber(self._scan_topic, + LaserScan, self._scan_callback) + + def tearDown(self): + """Called after every test. Cancel the scan subscription + """ + self._subscriber.unregister() + self._scan_q = None + self._subscriber = None + + def _scan_callback(self, scan_msg): + """Called every time a scan is received + """ + self._scan_q.put(scan_msg) + + def _wait_for_scan(self, timeout=1.0): + """Wait for a laser scan to be received + """ + # Use wall clock time for timeout + end_time = time.time() + timeout + while time.time() < end_time: + try: + return self._scan_q.get(True, 0.1) + except queue.Empty: + pass + return None + + def test_intensity_greater_than_256(self): + """Make sure stage_ros returns intensity values higher than 256 + """ + scan = self._wait_for_scan() + self.assertIsNotNone(scan) + self.assertGreater(max(scan.intensities), 256.9) + + +if __name__ == '__main__': + import rosunit + rosunit.unitrun(PKG, 'test_intensity', RangerIntensityTests) diff --git a/src/stage_ros_mod_tf/test/intensity_test.xml b/src/stage_ros_mod_tf/test/intensity_test.xml new file mode 100644 index 0000000..3a35e49 --- /dev/null +++ b/src/stage_ros_mod_tf/test/intensity_test.xml @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/src/stage_ros_mod_tf/world/erratic-inc.world b/src/stage_ros_mod_tf/world/erratic-inc.world new file mode 100644 index 0000000..99d2c41 --- /dev/null +++ b/src/stage_ros_mod_tf/world/erratic-inc.world @@ -0,0 +1,26 @@ +# A simple robot for inclusion in other world files + +define topurg ranger +( + sensor + ( + range [ 0.0 30.0 ] + fov 270.25 + samples 1081 + ) + # generic model properties + color "black" + size [ 0.05 0.05 0.1 ] +) + +define erratic position +( + size [ 0.35 0.35 0.25 ] + origin [ -0.05 0 0 0 ] + gui_nose 1 + drive "diff" + topurg + ( + pose [ 0 0 0 0 ] + ) +) diff --git a/src/stage_ros_mod_tf/world/intense.world b/src/stage_ros_mod_tf/world/intense.world new file mode 100644 index 0000000..a14a41e --- /dev/null +++ b/src/stage_ros_mod_tf/world/intense.world @@ -0,0 +1,63 @@ +# A world containing objects with varying ranger insensity values + +include "erratic-inc.world" + +resolution 0.02 +interval_sim 100 + +define block model +( + size [0.5 0.5 0.5] + gui_nose 0 +) + +erratic +( + pose [ 0 0 0 0 ] + name "era" + color "blue" +) + +# Insert blocks with different intensity values +block +( + pose [ 1.0 -3 0 0 ] + ranger_return 0 + color "gray0" +) +block +( + pose [ 1.0 -2 0 0 ] + ranger_return 50 + color "gray5" +) +block +( + pose [ 1.0 -1 0 0 ] + ranger_return 100 + color "gray10" +) +block +( + pose [ 1.0 0 0 0 ] + ranger_return 250 + color "gray25" +) +block +( + pose [ 1.0 1 0 0 ] + ranger_return 4000 + color "gray40" +) +block +( + pose [ 1.0 2 0 0 ] + ranger_return 80000 + color "gray80" +) +block +( + pose [ 1.0 3 0 0 ] + ranger_return 1000000 + color "gray100" +) diff --git a/src/stage_ros_mod_tf/world/willow-erratic.world b/src/stage_ros_mod_tf/world/willow-erratic.world new file mode 100644 index 0000000..6e341d9 --- /dev/null +++ b/src/stage_ros_mod_tf/world/willow-erratic.world @@ -0,0 +1,83 @@ +window +( + size [ 635 666 ] # in pixels + scale 22.971 # pixels per meter + center [ -20.306 21.679 ] + rotate [ 0.000 0.000 ] + + show_data 1 # 1=on 0=off +) + + +define block model +( + size [0.500 0.500 0.500] + gui_nose 0 +) + +define topurg ranger +( + sensor( + range [ 0.0 30.0 ] + fov 270.25 + samples 1081 + ) + + # generic model properties + color "black" + size [ 0.050 0.050 0.100 ] +) + +define erratic position +( + #size [0.415 0.392 0.25] + size [0.350 0.350 0.250] + origin [-0.050 0.000 0.000 0.000] + gui_nose 1 + drive "diff" + topurg(pose [ 0.050 0.000 0.000 0.000 ]) +) + +define floorplan model +( + # sombre, sensible, artistic + color "gray30" + + # most maps will need a bounding box + boundary 1 + + gui_nose 0 + gui_grid 0 + + gui_outline 0 + gripper_return 0 + fiducial_return 0 + ranger_return 1.000 +) + +# set the resolution of the underlying raytrace model in meters +resolution 0.02 + +interval_sim 100 # simulation timestep in milliseconds + + +window +( + size [ 745 448 ] + + rotate [ 0.000 -1.560 ] + scale 28.806 +) + +# load an environment bitmap +floorplan +( + name "willow" + bitmap "willow-full.pgm" + size [54.000 58.700 0.500] + pose [ -29.350 27.000 0.000 90.000 ] +) + +# throw in a robot +erratic( pose [ -11.277 23.266 0.000 180.000 ] name "era" color "blue") +block( pose [ -13.924 25.020 0.000 180.000 ] color "red") diff --git a/src/stage_ros_mod_tf/world/willow-four-erratics-multisensor.world b/src/stage_ros_mod_tf/world/willow-four-erratics-multisensor.world new file mode 100644 index 0000000..ee10fef --- /dev/null +++ b/src/stage_ros_mod_tf/world/willow-four-erratics-multisensor.world @@ -0,0 +1,87 @@ +define block model +( + size [0.5 0.5 0.5] + gui_nose 0 +) + +define topurg ranger +( + sensor( + range [ 0.0 30.0 ] + fov 270.25 + samples 1081 + ) + + # generic model properties + color "black" + size [ 0.05 0.05 0.1 ] +) + +define mycamera camera +( + range [ 0.2 8.0 ] + resolution [ 100 100 ] + fov [ 70 40 ] + pantilt [ 0 0 ] + alwayson 1 +) + +define erratic position +( + #size [0.415 0.392 0.25] + size [0.35 0.35 0.25] + origin [-0.05 0 0 0] + gui_nose 1 + drive "diff" + topurg(pose [ 0.050 0.000 0 0.000 ]) + topurg(pose [ -0.050 0.000 0 180.000 ]) + mycamera(pose [ 0 0 0 90.0 ]) + mycamera(pose [ 0 0 0 -90.0 ]) +) + +define floorplan model +( + # sombre, sensible, artistic + color "gray30" + + # most maps will need a bounding box + boundary 1 + + gui_nose 0 + gui_grid 0 + + gui_outline 0 + gripper_return 0 + fiducial_return 0 + laser_return 1 +) + +# set the resolution of the underlying raytrace model in meters +resolution 0.02 + +interval_sim 100 # simulation timestep in milliseconds + + +window +( + size [ 745.000 448.000 ] + + rotate [ 0.000 -1.560 ] + scale 28.806 +) + +# load an environment bitmap +floorplan +( + name "willow" + bitmap "willow-full.pgm" + size [54.0 58.7 0.5] + pose [ -29.350 27.000 0 90.000 ] +) + +# throw in two robots +erratic( pose [ -11.277 23.266 0 180.000 ] name "era" color "blue") +erratic( pose [ -13.277 22.266 0 180.000 ] name "era2" color "blue") +erratic( pose [ -13.277 23.266 0 180.000 ] name "era3" color "blue") +erratic( pose [ -11.277 22.266 0 180.000 ] name "era4" color "blue") +block( pose [ -13.924 25.020 0 180.000 ] color "red") diff --git a/src/stage_ros_mod_tf/world/willow-four-erratics.world b/src/stage_ros_mod_tf/world/willow-four-erratics.world new file mode 100644 index 0000000..bb9405c --- /dev/null +++ b/src/stage_ros_mod_tf/world/willow-four-erratics.world @@ -0,0 +1,75 @@ +define block model +( + size [0.5 0.5 0.5] + gui_nose 0 +) + +define topurg ranger +( + sensor( + range [ 0.0 30.0 ] + fov 270.25 + samples 1081 + ) + + # generic model properties + color "black" + size [ 0.05 0.05 0.1 ] +) + +define erratic position +( + #size [0.415 0.392 0.25] + size [0.35 0.35 0.25] + origin [-0.05 0 0 0] + gui_nose 1 + drive "diff" + topurg(pose [ 0.050 0.000 0 0.000 ]) +) + +define floorplan model +( + # sombre, sensible, artistic + color "gray30" + + # most maps will need a bounding box + boundary 1 + + gui_nose 0 + gui_grid 0 + + gui_outline 0 + gripper_return 0 + fiducial_return 0 + laser_return 1 +) + +# set the resolution of the underlying raytrace model in meters +resolution 0.02 + +interval_sim 100 # simulation timestep in milliseconds + + +window +( + size [ 745.000 448.000 ] + + rotate [ 0.000 -1.560 ] + scale 28.806 +) + +# load an environment bitmap +floorplan +( + name "willow" + bitmap "willow-full.pgm" + size [54.0 58.7 0.5] + pose [ -29.350 27.000 0 90.000 ] +) + +# throw in two robots +erratic( pose [ -11.277 23.266 0 180.000 ] name "era" color "blue") +erratic( pose [ -13.277 22.266 0 180.000 ] name "era2" color "blue") +erratic( pose [ -13.277 23.266 0 180.000 ] name "era3" color "blue") +erratic( pose [ -11.277 22.266 0 180.000 ] name "era4" color "blue") +block( pose [ -13.924 25.020 0 180.000 ] color "red") diff --git a/src/stage_ros_mod_tf/world/willow-full.pgm b/src/stage_ros_mod_tf/world/willow-full.pgm new file mode 100644 index 0000000..b16f1a4 --- /dev/null +++ b/src/stage_ros_mod_tf/world/willow-full.pgm @@ -0,0 +1,11 @@ +P5 +#Created with The GIMP +540 587 +255 +3U99-+3U΀΀ŒےU@΀%U+Ϊ@UD3Ù@KfUc9ffx@@3f9%Ϊ7UkM Ι /3@U @ `@-(Ι7MՀ3`%n@kF%U"FUǝۀ;fr@0+/+n @+ưUr1Un%UDcU U`@Gl+9FcMfn9np@UUU3`k3+33+@Up ΀vUtf@ΪffU(@Ϊb@_U\΀cK3Y΀Up΀%U@U23UUf33ffUf̀tEzooUlPk|ofHj|]lPY^^PIOXU9.HXH @nlvűغI̷\f@\ 39doNfPUSrZeer˹Ǻf`+UU+bIfybslUU۪љdz1Rp=En(f3@++Uj{Y{pz{sawk{wscwkӵU`ǎǷMfUUUUf]UFrrf]`؎ْDZ^aۣIUnnU+Q̪àwc\r3wҴժo{wU0ʱؤݹժ3UUUUr@UU΀@I`%0+F]yFvO\O@U+I{ۀICfUfN@̀rY΀UpI̻yz@fœbyRiNE>/Ufr`@t㧀]nYUUZ=N-)"-'"0 +΀@33cnUUPk@UUUUfe tUUcF@Uc΀3;tf@`K@UU˙pwk||Ì`UM%@3UUUfU73+fU3U)΀U)U@@HnfΪۻ̐vwpsffl`Kc%-II7/ࠊr3;@@3@@@9@ If @U3MIcfՀ@@@@@@3U%InժU̒تŒ 9UfU@շK%Z3@U#UΙ̪΀U@@%sUU΀΀rUn3tfUΎ 3 I\Iff΀1v̪IU 3E]΀r IF@9U13@U0w@fn(+nnU93`f9Uq̊۝(7虪UtG]UO7]Uݪ(3΀3]K3`n/UӊdPRts@IDٲ+II@@@UմI/(+/@rffn3@@% ]]3D- @+FM%cΪ;ΪŰ\UU/ΪU@773n3FrUU9Ui%F9UI3|l3 tUU@U3cɌc@M(tIIU̪UU @;U@΀\f%3U;9@`@Us ΀΀U9U%΀UU@UnΆU93΀`Հ++P"n΀@UU@/Ǫ;@@΀`fn+ Ţ@، \9`΀+r3; %΀ݙnDI3f̀D9@3UU 9n@U(%`rѱ΀e@UUUvn@@c9΀r%U@I%MU`f+UIf΀r@U@3%%+l /΀UUnIUI@\Ґ67M``fM3@n̷f`r_ZϛP\sk@rUfUvtftf ΀%΀U۠U99%+̪̒Uت/΀΀UnIUۀ΀IUUUM\99΀wv3nI@zZ3]3U@ Mr3n@`ՀUI΀IUU ̀`IU0U@c"΀ O3ǪfDf+\+Ui3@U@3nfUUKΪn@]@U@UUZU9`U`UΪU@΀U΀n@+%΀UՀ̝@Ϊr @@UU]΀UU]3Ůp΀΀crU@fUknIU@%@U`Ϊ\n@@@33@U@tf΀\΀UnUrUU΀U΀I`΀@`r;f̻΀f0UU iǀk(`Հft7fZ@΀F΀΀U@t]n %3+%%(%iUfFr΀౪ΊfUtrŎU3U΀tۣf@UU]`΀vUUUU̪fUG8)I(-'   # 1+ )+3"7Ic@ffMfrՙUU/Ǚf+33̪U3U^ՀOrnot/ю`3@@U̷ cUzY/ՀI%+UtfiUcۀ+@UrǪ3 M`v@pΪ3f@@@تۺ%wη]`9 ΀΀3I̪UUUfnр3΀;|U]3(M3`f΀pUPKMRpFrnw7-ΪUnP΀bf9%t<3F@f΀UU@36qnn|Un@nf@f93@`@+;/UU@D++fΪUUU(U<úxw{ۖṯժ̀U@΀9f tW@rï@+TEK9I@UUttFU@ffIfUKUnfcՀF@]`UU"Mn@Ί;zzujjrdtU`@fD@΀M;Uz+%̤U@ +P3ξvcKUI=IfkI9U\I@·GU xU\f0"6fɀίܢ+Ut6G9II@U Mrrk]]UUUrI@%UKXxR+@@M6U@;C0a3MMU3x@nG%Uf9cx`Mf3Z3f`RU`Ur]UUID9@M@%Ix0 9@\FrMn``@rUU`@@3;t̡U̷MU"%5C`@9TC9@nFFrЇ%UrG`Uf3v] vov]n@]vawv"tO Owrvnk@%nՏxIfU;zOc/ ;I(MFUnI@M"#v+Ir΀wݤ@lΪU93DŽ[9\fnΪk@@9M(QK΀\++ tvkr+3+U+-@]+΀t'Uf3rR@IYxvd++@Ύ</+NJ@f%@zsUcpxrnU%0(((Ϊ5@%@Un@iu3wv%΀΀ Co΀%Ϊ@@@Fiߍ eU@ΪUky{%dՒMγ7Ut΀U7`% ]`Du@ZU7΀۪%UdU̒RU`IU"+t +̀+iDsd` YU[FΙ7w6e@΀fPc̕c3΀{r@Un)O+tK@wfΙvM3CU@+΀U3rUFUllnM΀jrd0U3UUmwΪ` Kzrr@@@3UUUM+o0++rw+U뵻rIU3@UUIt΀F;U/`sx3`Uے%p홖]M@9ck΀<ÀUdUtU\\URǀf+ݭ yt]+nwsU3)ǀ3f`wIUNJ9nU3U@UnMkU +äfrvrnǎUrf@M\Or@U]'i̙U挱nfUIUU̎nC>% @3R3c=Yf3@% 3f+3@@U@U\@"ڷU#(nf@UI̠@΀FUUU΀Mpv|@w췷UU@΀fҤ{{Uժ+%39+U+ (+I%GΪ+]+۱l6\9+UU+@UfUΪ@zx΀3OU{c΀_uR rŌIUU`UcoΪ " +I+@3OΪU΀o^CIΪ@I@3nir /01I\F΀΀6ɻǪ@ryI wUΪ9;dIwfGQ3O`U@@xU ̴@+i ΀p@fΪ̙f:swU ΀z(f@Ϊy2vU΀+f"~hb@@΀tUMUUǙUUf@k`;΀bt3\UM9tUU@ UU\fc RUUq/%KժΪ쳀Frc@ɪ@+ݮDUU3Uf@3R0%ՀF9fU -IUG?UKU΀U+̷DMf΀(+c@3+lcIrM΀GU@+U't̀U``f" M΀\3@UUnI]I౒U@ 3tD O@+m@+Ս%@+@`@3+̴U3D+UU3-fe۪΀v:UF@ UUv̙΀(d@93w\(Iv ΀%33GK< +D33K΀Ϊ`UfU-#ɻ"Ui)պΪUr@ "  & dvf3% +++U3v΀@+̪tF3+Lc3UU`Uf@@U̪\/33fUUfIf΀I@3U@IU@UU΀ n@ 3f`UU΀U+fժ+Ϊ΀/vU/@+U@fΪUUȣr9%IUUk{U ǵoID@cRKaU`̙΀ΪcFUrrf yfUUnU-%ÐUUU933΀UU9+k@ݰ@@`U%(UUF9M@^`+fU+UU΀OfǷD΀UvxUU PDΪI`U`/Z+"c@Uk@3p΀Ϊ΀΀UK@)pUU<U`Z 6dPUUU+]F;F@bfU3ΙUpZUI1΀v@@f= 37fNUk3Uf<3@fI\΀vn+Ϊf@@շ3KfÙn3fU@;ժ@@``Ur΀UP΀@ZU3ÌrÀ9@΀@U`cU@Fd<UI+rU9՝IUU3f fUK Ό@3+U@7MU</ǙnfIۊcΪѺUf+%^nUt@@U/΀%ۙDfk3nΪ+@%U%%@@@+@3f33ժշt`+U3+n%U@UxrrI3f n(fU@U΀wUrU΀΀ΪΪ \ No newline at end of file