Add wireless receiver sensor

CMakeLists.txt

@@ -0,0 +1 @@
+/opt/ros/indigo/share/catkin/cmake/toplevel.cmake

hector_gazebo_plugins/CMakeLists.txt

@@ -94,6 +94,20 @@ add_dependencies(hector_gazebo_ros_sonar ${PROJECT_NAME}_gencfg)
 add_library(hector_servo_plugin src/servo_plugin.cpp)
 target_link_libraries(hector_servo_plugin ${Boost_LIBRARIES} ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES})
 
+add_library(hector_gazebo_ros_wireless_receiver src/gazebo_ros_wireless_receiver.cpp)
+target_link_libraries(hector_gazebo_ros_wireless_receiver ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES})
+add_dependencies(hector_gazebo_ros_wireless_receiver ${PROJECT_NAME}_gencfg)
+
+add_library(hector_gazebo_ros_velodyne src/gazebo_ros_velodyne.cpp)
+target_link_libraries(hector_gazebo_ros_velodyne ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES})
+add_dependencies(hector_gazebo_ros_velodyne ${PROJECT_NAME}_gencfg)
+
+
+add_library(gazebo_ros_create src/gazebo_ros_create.cpp)
+target_link_libraries(gazebo_ros_create  ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
+add_dependencies(gazebo_ros_create ${PROJECT_NAME}_gencfg)
+
+
 #############
 ## Install ##
 #############

hector_gazebo_plugins/include/hector_gazebo_plugins/gazebo_ros_create.h

@@ -0,0 +1,106 @@
+#ifndef GAZEBO_ROS_CREATE_H
+#define GAZEBO_ROS_CREATE_H
+
+#include <gazebo/msgs/msgs.hh>
+#include <gazebo/physics/physics.hh>
+#include <gazebo/physics/PhysicsTypes.hh>
+#include <gazebo/sensors/SensorTypes.hh>
+#include <gazebo/transport/TransportTypes.hh>
+#include <gazebo/transport/transport.hh>
+#include <gazebo/common/Time.hh>
+#include <gazebo/common/Plugin.hh>
+#include <gazebo/common/Events.hh>
+
+#include <nav_msgs/Odometry.h>
+#include <geometry_msgs/TwistWithCovariance.h>
+#include <geometry_msgs/PoseWithCovariance.h>
+#include <tf/transform_broadcaster.h>
+#include <ros/ros.h>
+
+namespace gazebo
+{
+  class GazeboRosCreate : public ModelPlugin
+  {
+    public:
+      GazeboRosCreate();
+      virtual ~GazeboRosCreate();
+
+      virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf );
+
+      virtual void UpdateChild();
+
+    private:
+
+      void UpdateSensors();
+      void OnContact(ConstContactsPtr &contact);
+      void OnCmdVel( const geometry_msgs::TwistConstPtr &msg);
+
+
+      /// Parameters
+      std::string node_namespace_;
+      std::string left_wheel_joint_name_;
+      std::string right_wheel_joint_name_;
+      std::string front_castor_joint_name_;
+      std::string rear_castor_joint_name_;
+      std::string base_geom_name_;
+
+      /// Separation between the wheels
+      float wheel_sep_;
+
+      /// Diameter of the wheels
+      float wheel_diam_;
+
+      ///Torque applied to the wheels
+      float torque_;
+
+
+      ros::NodeHandle *rosnode_;
+      //ros::Service operating_mode_srv_;
+      //ros::Service digital_output_srv_;
+
+      ros::Publisher sensor_state_pub_;
+      ros::Publisher odom_pub_;
+      ros::Publisher joint_state_pub_;
+
+      ros::Subscriber cmd_vel_sub_;
+
+      physics::WorldPtr my_world_;
+      physics::ModelPtr my_parent_;
+
+      /// Speeds of the wheels
+      float *wheel_speed_;
+
+      // Simulation time of the last update
+      common::Time prev_update_time_;
+      common::Time last_cmd_vel_time_;
+
+      float odom_pose_[3];
+      float odom_vel_[3];
+
+      bool set_joints_[4];
+      physics::JointPtr joints_[4];
+      physics::CollisionPtr base_geom_;
+
+      sensors::RaySensorPtr left_cliff_sensor_;
+      sensors::RaySensorPtr leftfront_cliff_sensor_;
+      sensors::RaySensorPtr rightfront_cliff_sensor_;
+      sensors::RaySensorPtr right_cliff_sensor_;
+      sensors::RaySensorPtr wall_sensor_;
+
+      tf::TransformBroadcaster transform_broadcaster_;
+      sensor_msgs::JointState js_;
+
+      create_node::TurtlebotSensorState sensor_state_;
+
+      void spin();
+      boost::thread *spinner_thread_;
+
+      //event::ConnectionPtr contact_event_;
+      transport::NodePtr gazebo_node_;
+      transport::SubscriberPtr contact_sub_;
+
+      // Pointer to the update event connection
+      event::ConnectionPtr updateConnection;
+  };
+}
+#endif

hector_gazebo_plugins/include/hector_gazebo_plugins/gazebo_ros_velodyne.h

@@ -0,0 +1,84 @@
+//=================================================================================================
+// Copyright (c) 2012, Johannes Meyer, TU Darmstadt
+// All rights reserved.
+
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the Flight Systems and Automatic Control group,
+//       TU Darmstadt, nor the names of its contributors may be used to
+//       endorse or promote products derived from this software without
+//       specific prior written permission.
+
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//=================================================================================================
+
+#ifndef HECTOR_GAZEBO_PLUGINS_GAZEBO_ROS_VELODYNE_H
+#define HECTOR_GAZEBO_PLUGINS_GAZEBO_ROS_VELODYNE_H
+
+#include <gazebo/common/Plugin.hh>
+
+#include <ros/callback_queue.h>
+#include <ros/ros.h>
+
+#include <sensor_msgs/PointCloud2.h>
+#include <hector_gazebo_plugins/sensor_model.h>
+#include <hector_gazebo_plugins/update_timer.h>
+#include <gazebo/plugins/RayPlugin.hh>
+
+#include <dynamic_reconfigure/server.h>
+
+namespace gazebo
+{
+
+class GazeboRosVelodyne : public SensorPlugin
+{
+public:
+  GazeboRosVelodyne();
+  virtual ~GazeboRosVelodyne();
+
+protected:
+  virtual void Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf);
+  virtual void Reset();
+  virtual void Update();
+
+private:
+  /// \brief The parent World
+  physics::WorldPtr world;
+
+  sensors::RaySensorPtr sensor_;
+
+  ros::NodeHandle* node_handle_;
+  ros::Publisher publisher_;
+
+  sensor_msgs::PointCloud2 cloud_msg_;
+
+  std::string namespace_;
+  std::string topic_;
+  std::string frame_id_;
+  int updateRate_;
+  int  point_step;
+  SensorModel sensor_model_;
+
+  UpdateTimer updateTimer;
+  event::ConnectionPtr updateConnection;
+
+  boost::shared_ptr<dynamic_reconfigure::Server<SensorModelConfig> > dynamic_reconfigure_server_;
+};
+
+} // namespace gazebo
+
+#endif // HECTOR_GAZEBO_PLUGINS_GAZEBO_ROS_SONAR_H

hector_gazebo_plugins/include/hector_gazebo_plugins/gazebo_ros_wireless_receiver.h

@@ -0,0 +1,93 @@
+//=================================================================================================
+// Copyright (c) 2012, Johannes Meyer, TU Darmstadt
+// All rights reserved.
+
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the Flight Systems and Automatic Control group,
+//       TU Darmstadt, nor the names of its contributors may be used to
+//       endorse or promote products derived from this software without
+//       specific prior written permission.
+
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//=================================================================================================
+
+#ifndef HECTOR_GAZEBO_PLUGINS_GAZEBO_ROS_WIRELESS_RECEIVER_H
+#define HECTOR_GAZEBO_PLUGINS_GAZEBO_ROS_WIRELESS_RECEIVER_H
+
+#include <gazebo/common/Plugin.hh>
+
+#include <ros/callback_queue.h>
+#include <ros/ros.h>
+
+#include <sensor_msgs/PointCloud.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <hector_gazebo_plugins/sensor_model.h>
+#include <hector_gazebo_plugins/update_timer.h>
+
+
+#include <dynamic_reconfigure/server.h>
+#include <gazebo/sensors/Sensor.hh>
+
+namespace gazebo
+{
+
+class GazeboRosWirelessReceiver : public SensorPlugin
+{
+public:
+  GazeboRosWirelessReceiver();
+  virtual ~GazeboRosWirelessReceiver();
+
+protected:
+  virtual void Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf);
+  virtual void Reset();
+  virtual void Update();
+
+private:
+  /// \brief The parent World
+  physics::WorldPtr world;
+
+  sensors::SensorPtr sensor_;
+
+  ros::NodeHandle* node_handle_;
+
+  ros::Publisher transmitter_pub_;
+  ros::Publisher rss_pub_;
+  ros::Publisher AoA_pub_;
+  ros::Publisher receiver_pub_;
+  int transmitter_count_;
+
+
+  /// \brief Parent entity of this sensor
+  physics::EntityPtr parentEntity;
+
+  std::string namespace_;
+  std::string topic_;
+  std::string frame_id_;
+
+  SensorModel rss_sensor_model_;
+  SensorModel3 AoA_sensor_model_;
+
+  UpdateTimer updateTimer;
+  event::ConnectionPtr updateConnection;
+
+  boost::shared_ptr<dynamic_reconfigure::Server<SensorModelConfig> > dynamic_reconfigure_server_;
+};
+
+} // namespace gazebo
+
+#endif // HECTOR_GAZEBO_PLUGINS_GAZEBO_ROS_SONAR_H