Charles padawan

ca_gazebo/CMakeLists.txt

@@ -30,11 +30,15 @@ else()
 		${GAZEBO_INCLUDE_DIRS}
   )
 
-	add_library(create_bumper_plugin src/create_bumper_plugin.cpp)
+  add_library(create_bumper_plugin src/create_bumper_plugin.cpp)
 	add_dependencies(create_bumper_plugin ${catkin_EXPORTED_TARGETS})
 	target_link_libraries(create_bumper_plugin
 				${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
 
+  add_library(model_pose_publisher_plugin src/model_pose_publisher_plugin.cpp)
+  target_link_libraries(model_pose_publisher_plugin
+        ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
+
   install(DIRECTORY launch worlds models
     DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
 

ca_gazebo/launch/create_empty_world.launch

@@ -1,20 +1,21 @@
 <launch>
 	<arg name="env" default="empty"/>
-
-  	<arg name="paused" value="$(optenv PAUSED false)"/>
+	<arg name="paused" value="$(optenv PAUSED false)"/>
 	<arg name="gui" value="$(optenv GUI true)"/>
 	<arg name="debug" value="$(optenv DEBUG false)"/>
 
+  <arg name="localization" default="$(optenv LOCALIZATION none)"/>
+
 	<!-- Launch Gazebo world -->
 	<include file="$(find gazebo_ros)/launch/empty_world.launch">
 		<arg name="debug" value="$(arg debug)" />
 		<arg name="gui" value="$(arg gui)" />
 		<arg name="paused" value="$(arg paused)"/>
 		<arg name="use_sim_time" value="true"/>
-		<arg name="verbose" value="true"/>
+		<arg name="verbose" value="$(arg debug)"/>
 		<arg name="world_name" value="$(find ca_gazebo)/worlds/$(arg env).world"/>
 	</include>
-	
+
 	<group if="$(eval str(arg('env'))=='empty')">
 			<param name="create1/amcl/initial_pose_x" value="-3"/>
 			<param name="create1/amcl/initial_pose_y" value="4"/>
@@ -26,10 +27,13 @@
 
 
 	<!-- Spawn robot/s -->
-	<include file="$(find ca_gazebo)/launch/spawn_multirobot.launch"/>
+	<include file="$(find ca_gazebo)/launch/spawn_multirobot.launch">
+		<arg name="localization" value="$(arg localization)" />
+	</include>
 
   <!-- map_server -->
-  <include file="$(find ca_move_base)/launch/map_server.launch">
+  <include if="$(eval str(arg('localization'))=='amcl')"
+			file="$(find ca_move_base)/launch/map_server.launch">
 	<arg name="env" value="$(arg env)"/>
   </include>
 

ca_gazebo/launch/ekuthon_coop_robots.launch

@@ -0,0 +1,13 @@
+<launch>
+  <param name="create1/amcl/initial_pose_x" value="0.29"/>
+  <param name="create1/amcl/initial_pose_y" value="0.20"/>
+  <param name="create1/amcl/initial_pose_a" value="-1.57"/>
+
+  <param name="create2/amcl/initial_pose_x" value="0.29"/>
+  <param name="create2/amcl/initial_pose_y" value="0.55"/>
+  <param name="create2/amcl/initial_pose_a" value="-1.57"/>
+
+  <include file="$(find ca_gazebo)/launch/create_empty_world.launch">
+    <arg name="env" value="two_rooms"/>
+  </include>
+</launch>

ca_gazebo/launch/spawn_multirobot.launch

@@ -3,7 +3,7 @@
   <arg name="nr" default="$(optenv NUM_ROBOTS 1)"/>
   <arg name="rviz" default="$(optenv RVIZ true)"/>
   <arg name="localization" default="$(optenv LOCALIZATION none)"/>
-  
+
   <!-- Start 1 robot -->
   <group ns="$(eval 'create' + str(arg('nr')))">
     <include file="$(find ca_gazebo)/launch/spawn_create.launch">
@@ -30,5 +30,6 @@
   <include file="$(find ca_gazebo)/launch/spawn_multirobot.launch" if="$(eval arg('nr') - 1 > 0)">
     <arg name="nr" value="$(eval arg('nr') - 1)"/>
     <arg name="rviz" value="false"/>
+    <arg name="localization" value="$(arg localization)" />
   </include>
-</launch>
+</launch>

ca_gazebo/src/model_pose_publisher_plugin.cpp

@@ -0,0 +1,80 @@
+#include <ros/ros.h>
+#include <sdf/sdf.hh>
+#include <ignition/math/Pose3.hh>
+#include <ignition/math/Vector3.hh>
+#include "gazebo/gazebo.hh"
+#include "gazebo/common/Plugin.hh"
+#include "gazebo/msgs/msgs.hh"
+#include "gazebo/physics/physics.hh"
+#include "gazebo/transport/transport.hh"
+#include "std_msgs/String.h"
+#include "geometry_msgs/Pose.h"
+
+static const ros::Duration update_rate = ros::Duration(1); // 1 Hz
+namespace gazebo
+{
+class ModelPosePublisherPlugin : public ModelPlugin
+{
+
+    public: void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/)
+    {
+      // Make sure the ROS node for Gazebo has already been initialized
+      if (!ros::isInitialized())
+      {
+        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;
+      }
+      ROS_INFO("Model pose publisher started!");
+      ROS_INFO("model Name = %s", _parent->GetName().c_str());
+
+      // Store the pointer to the model
+      this->model = _parent;
+
+      // Listen to the update event. This event is broadcast every
+      // simulation iteration.
+      this->updateConnection = event::Events::ConnectWorldUpdateBegin(
+          std::bind(&ModelPosePublisherPlugin::OnUpdate, this));
+      this->prev_update_time_ = ros::Time::now();
+
+      this->rosnode_.reset(new ros::NodeHandle("ModelPose"));
+      this->pub_ = this->rosnode_->advertise<geometry_msgs::Pose>("/ca_gazebo/model_pose", 100);
+    }
+
+    // Called by the world update start event
+    public: void OnUpdate()
+    {
+      if ((ros::Time::now() - this->prev_update_time_) < update_rate) {
+       return;
+      }
+
+      geometry_msgs::Pose msg;
+      ignition::math::Pose3d pose = this->model->GetLink("link")->WorldPose();
+      msg.position.x = pose.Pos().X();
+      msg.position.y = pose.Pos().Y();
+      msg.position.z = pose.Pos().Z();
+
+      msg.orientation.x = pose.Rot().X();
+      msg.orientation.y = pose.Rot().Y();
+      msg.orientation.z = pose.Rot().Z();
+      msg.orientation.w = pose.Rot().W();
+
+      this->pub_.publish(msg);
+
+      this->prev_update_time_ = ros::Time::now();
+    }
+
+    private: std::shared_ptr<ros::NodeHandle> rosnode_;
+    private: ros::Publisher pub_;
+    // Pointer to the model
+    private: physics::ModelPtr model;
+
+    private: ros::Time prev_update_time_;
+
+    // Pointer to the update event connection
+    private: event::ConnectionPtr updateConnection;
+   };
+
+   // Register this plugin with the simulator
+   GZ_REGISTER_MODEL_PLUGIN(ModelPosePublisherPlugin)
+}