diff --git a/labrob_control/CHANGELOG.rst b/labrob_control/CHANGELOG.rst index 324638e..529aa13 100644 --- a/labrob_control/CHANGELOG.rst +++ b/labrob_control/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package labrob_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Added scoring interface (topics and services) +* Contributors: Alex, fsuarez6 + 0.1.0 (2014-04-07) ------------------ * Initial release diff --git a/labrob_description/CHANGELOG.rst b/labrob_description/CHANGELOG.rst index a214d82..60ca1a7 100644 --- a/labrob_description/CHANGELOG.rst +++ b/labrob_description/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package labrob_description ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Added scoring interface (topics and services) +* Contributors: fsuarez6 + 0.1.0 (2014-04-07) ------------------ * Initial release diff --git a/labrob_gazebo/CHANGELOG.rst b/labrob_gazebo/CHANGELOG.rst index 6d64402..0083cfa 100644 --- a/labrob_gazebo/CHANGELOG.rst +++ b/labrob_gazebo/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package labrob_gazebo ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Added scoring interface (topics and services) +* Contributors: fsuarez6 + 0.1.0 (2014-04-07) ------------------ * Initial release diff --git a/labrob_msgs/CHANGELOG.rst b/labrob_msgs/CHANGELOG.rst new file mode 100644 index 0000000..8ab60a9 --- /dev/null +++ b/labrob_msgs/CHANGELOG.rst @@ -0,0 +1,8 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package labrob_msgs +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* Added scoring interface (topics and services) +* Contributors: fsuarez6 diff --git a/labrob_msgs/CMakeLists.txt b/labrob_msgs/CMakeLists.txt new file mode 100644 index 0000000..dee7421 --- /dev/null +++ b/labrob_msgs/CMakeLists.txt @@ -0,0 +1,23 @@ +cmake_minimum_required(VERSION 2.8.3) +project(labrob_msgs) + +find_package(catkin REQUIRED COMPONENTS + message_generation nav_msgs) + +add_service_files( + FILES + GetMapPercentage.srv +) + +generate_messages(DEPENDENCIES nav_msgs std_msgs) + +catkin_package( +CATKIN_DEPENDS + message_runtime +) + + +include_directories( + ${catkin_INCLUDE_DIRS} +) + diff --git a/labrob_msgs/package.xml b/labrob_msgs/package.xml new file mode 100644 index 0000000..4a3ffd6 --- /dev/null +++ b/labrob_msgs/package.xml @@ -0,0 +1,22 @@ + + + labrob_msgs + 0.1.0 + The labrob_msgs package + + Francisco Suarez-Ruiz + BSD + + http://www.romin.upm.es/wiki/ + https://github.com/fsuarez6/labrob + Francisco Suarez-Ruiz + + catkin + nav_msgs + message_generation + + nav_msgs + message_runtime + + + diff --git a/labrob_msgs/srv/GetMapPercentage.srv b/labrob_msgs/srv/GetMapPercentage.srv new file mode 100644 index 0000000..40247b4 --- /dev/null +++ b/labrob_msgs/srv/GetMapPercentage.srv @@ -0,0 +1,3 @@ +nav_msgs/OccupancyGrid map +--- +float32 percentage_covered diff --git a/labrob_worlds/CHANGELOG.rst b/labrob_worlds/CHANGELOG.rst new file mode 100644 index 0000000..48497d7 --- /dev/null +++ b/labrob_worlds/CHANGELOG.rst @@ -0,0 +1,8 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package labrob_worlds +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* Added scoring interface (topics and services) +* Contributors: fsuarez6 diff --git a/labrob_worlds/CMakeLists.txt b/labrob_worlds/CMakeLists.txt index f0bbb59..c6c06a9 100644 --- a/labrob_worlds/CMakeLists.txt +++ b/labrob_worlds/CMakeLists.txt @@ -5,6 +5,8 @@ find_package(catkin REQUIRED COMPONENTS gazebo_ros roscpp rospy + labrob_msgs + nav_msgs ) @@ -12,8 +14,12 @@ catkin_package( CATKIN_DEPENDS roscpp gazebo_ros + labrob_msgs + nav_msgs ) +catkin_python_setup() + find_package(gazebo REQUIRED) link_directories(${GAZEBO_LIBRARY_DIRS}) diff --git a/labrob_worlds/launch/crazy_maze.launch b/labrob_worlds/launch/crazy_maze.launch index eae99cd..092e22f 100644 --- a/labrob_worlds/launch/crazy_maze.launch +++ b/labrob_worlds/launch/crazy_maze.launch @@ -12,16 +12,11 @@ - - + + + + + + diff --git a/labrob_worlds/models/crazy_coins/model.sdf b/labrob_worlds/models/crazy_coins/model.sdf index d2654bf..7210f11 100644 --- a/labrob_worlds/models/crazy_coins/model.sdf +++ b/labrob_worlds/models/crazy_coins/model.sdf @@ -266,7 +266,9 @@ + coin_2 labrob + 10.0 1.570796327 diff --git a/labrob_worlds/package.xml b/labrob_worlds/package.xml index f83b0e5..4758f05 100644 --- a/labrob_worlds/package.xml +++ b/labrob_worlds/package.xml @@ -16,12 +16,14 @@ gazebo_ros roscpp rospy - geometry_msgs + labrob_msgs + nav_msgs gazebo_ros roscpp rospy - geometry_msgs + labrob_msgs + nav_msgs diff --git a/labrob_worlds/scripts/challenge_map_server.py b/labrob_worlds/scripts/challenge_map_server.py new file mode 100755 index 0000000..2ebf461 --- /dev/null +++ b/labrob_worlds/scripts/challenge_map_server.py @@ -0,0 +1,34 @@ +#!/usr/bin/env python +import rospy, math +from labrob_msgs.srv import GetMapPercentage + +from nav_msgs.msg import OccupancyGrid + +class ChallengeMapServer: + def __init__(self): + rospy.init_node('challenge_map_server') + + # Initial values + size_m2 = rospy.get_param('/map_size_m2', 400.0) + resolution = rospy.get_param('/map_resolution', 0.05) + self.size_map = size_m2/resolution + + # Advertise the service + s = rospy.Service('get_percent_map_completed', GetMapPercentage, self.handle_map) + + rospy.loginfo('Challenge begun. Map size: %d squares' % int(self.size_map)); + + def handle_map(self, req): + rospy.loginfo('Received map. Calculating Percentage Completed...'); + + n_squares_mapped = sum(1 for x in req.map.data if x >= 0) + percentage = float(n_squares_mapped)/float(self.size_map) + + rospy.loginfo('Map Complete: '+str(100.0 * percentage)+'%') + + return percentage + + +if __name__ == "__main__": + ChallengeMapServer() + rospy.spin() diff --git a/labrob_worlds/setup.py b/labrob_worlds/setup.py new file mode 100644 index 0000000..cf3dbd7 --- /dev/null +++ b/labrob_worlds/setup.py @@ -0,0 +1,11 @@ +#!/usr/bin/env python + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup( + packages=['labrob_worlds'], + package_dir={'': 'scripts'}) + +setup(**setup_args) diff --git a/labrob_worlds/src/coins_manager_plugin.cpp b/labrob_worlds/src/coins_manager_plugin.cpp index ac5c49d..c2dbbaf 100644 --- a/labrob_worlds/src/coins_manager_plugin.cpp +++ b/labrob_worlds/src/coins_manager_plugin.cpp @@ -38,6 +38,8 @@ * \desc Plugin for animation of all the coins within the maze */ #include +#include +#include #include @@ -62,12 +64,23 @@ class CoinsManager : public ModelPlugin physics::Link_V links_; physics::Joint_V joints_; + // ROS STUFF + ros::NodeHandle* rosnode_; + ros::Publisher coins_publisher_; + ros::Publisher complete_publisher_; + double spin_velocity_; double coin_radius_; int total_coins_; + bool course_complete_; + std::string plugin_namespace_; std::string robot_name_; + std::string goal_coin_; event::ConnectionPtr update_connection_; - common::Time last_time_; + // Update Rate + double update_rate_; + double update_period_; + common::Time last_update_time_; /* Constructor */ public: CoinsManager() : ModelPlugin() { @@ -76,6 +89,7 @@ class CoinsManager : public ModelPlugin /* Destructor */ public: virtual ~CoinsManager() { + delete rosnode_; event::Events::DisconnectWorldUpdateBegin(update_connection_); } @@ -91,13 +105,32 @@ class CoinsManager : public ModelPlugin // Set the coin radius coin_radius_ = 1.0; - // Set the coins spin velocity + // Read all the plugin parameters + robot_name_ = "labrob"; + if (!_sdf->HasElement("robot_name")) + ROS_WARN_STREAM("coins_manager_plugin missing parameter, using [" << robot_name_ << "]"); + else + robot_name_ = _sdf->GetElement("robot_name")->Get(); + + goal_coin_ = "coin_1"; + if (!_sdf->HasElement("goal_coin")) + ROS_WARN_STREAM("coins_manager_plugin missing parameter, using [" << goal_coin_ << "]"); + else + goal_coin_ = _sdf->GetElement("goal_coin")->Get(); + + update_rate_ = 100.0; + if (!_sdf->HasElement("update_rate")) + ROS_WARN("coins_manager_plugin missing , defaults to %f", update_rate_); + else + update_rate_ = _sdf->GetElement("update_rate")->Get(); + spin_velocity_ = 1.570796327; if (!_sdf->HasElement("spin_velocity")) - gzwarn << "coins_manager_plugin missing parameter, using [" << spin_velocity_ << "] rad."<< endl; + ROS_WARN_STREAM("coins_manager_plugin missing parameter, using [" << spin_velocity_ << "] rad/s"); else spin_velocity_ = _sdf->GetElement("spin_velocity")->Get(); + // Make spin the coins for (int i = 0; i < links_.size(); ++i) { // Randomize the spin direction @@ -105,44 +138,53 @@ class CoinsManager : public ModelPlugin links_[i]->SetAngularVel(math::Vector3(0, 0, spin_velocity_*spin_dir)); } total_coins_ = links_.size(); - gzdbg << "Found [" << total_coins_ << "] coins!" << endl; + ROS_DEBUG_STREAM("Found [" << total_coins_ << "] coins!"); // Make sure the ROS node for Gazebo has already been initialized if (!ros::isInitialized()) { - gzerr << "A ROS node for Gazebo has not been initialized, unable to load plugin. " - << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)" << endl; + ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. " + << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); return; } - // Reset Time - last_time_ = world_->GetSimTime(); + + plugin_namespace_ = "/"; + rosnode_ = new ros::NodeHandle(plugin_namespace_); + + // ROS: Setup publishers + coins_publisher_ = rosnode_->advertise("coins_remaining", 1); + complete_publisher_ = rosnode_->advertise("course_complete", 1); + + // Initialize update rate stuff + + if (update_rate_ > 0.0) + update_period_ = 1.0 / update_rate_; + else + update_period_ = 0.0; + last_update_time_ = world_->GetSimTime(); + // Listen to the update event. This event is broadcast every simulation iteration. update_connection_ = event::Events::ConnectWorldUpdateBegin( boost::bind(&CoinsManager::UpdateChild, this)); - - // Wait for the robot to be spawn - robot_name_ = "labrob"; - if (!_sdf->HasElement("robot_name")) - gzwarn << "coins_manager_plugin missing parameter, using [" << robot_name_ << "] rad."<< endl; - else - robot_name_ = _sdf->GetElement("robot_name")->Get(); + + ROS_INFO("Starting CoinsManager Plugin (ns = %s)!", plugin_namespace_.c_str()); + course_complete_ = false; - BlueMsg("coins_manager_plugin successfully loaded!"); } /* Update the plugin */ - // TODO: Need to publish interest information to ROS: Total coins, collected coins, reached the goal? private: void UpdateChild() { math::Pose robot_pose; math::Vector3 position_diff; double distance; common::Time current_time = world_->GetSimTime(); + double seconds_since_last_update = (current_time - last_update_time_).Double(); - if (current_time > last_time_) + if (seconds_since_last_update > update_period_) { // Put the 'intelligence' in here! - if (!robot_model_) + if (!robot_model_) // Wait for the robot to be spawn robot_model_ = world_->GetModel(robot_name_); else { @@ -150,27 +192,31 @@ class CoinsManager : public ModelPlugin robot_pose = robot_model_->GetWorldPose(); for (int i = 0; i < links_.size(); ++i) { - // TODO: Should use GetBoundingBox() and check the intersection + // TODO: Should use links_[i]->GetBoundingBox() and check the intersection math::Pose coin_pose = links_[i]->GetWorldPose(); position_diff = coin_pose.pos - robot_pose.pos; distance = sqrt(pow(position_diff.x, 2) + pow(position_diff.y, 2)); // If the robot is close enough delete the coin if (distance < coin_radius_) { + // Check if We got the goal coin + if (goal_coin_.compare(links_[i]->GetName()) == 0) + course_complete_ = true; model_->RemoveChild(links_[i]); links_.erase(links_.begin() + i); } } - } - last_time_ = current_time; + } + // Publish topics + std_msgs::Int32 coins_msg; + coins_msg.data = links_.size(); + coins_publisher_.publish(coins_msg); + std_msgs::Bool complete_msg; + complete_msg.data = course_complete_; + complete_publisher_.publish(complete_msg); + last_update_time_+= common::Time(update_period_); } } - - /* Custom log messages */ - private: void BlueMsg(std::string msg) { - gzmsg << "\033[94m" << msg << "\033[0m" << endl; } - private: void GreenMsg(std::string msg) { - gzmsg << "\033[92m" << msg << "\033[0m" << endl; } }; GZ_REGISTER_MODEL_PLUGIN(CoinsManager)