Merge pull request #53 from open-rdc/add-gazebo-pkg

Moved from the icart_mini_gazebo package

icart_mini_gazebo/CHANGELOG.rst

@@ -0,0 +1,61 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package icart_mini_gazebo
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+Forthcoming
+-----------
+* Merge pull request `#9 <https://github.com/open-rdc/icart_mini_gazebo/issues/9>`_ from open-rdc/fix-depend-pkg
+  Add the dependency packages
+* Add the dependency package
+* Fixed the rosdep error
+* Merge pull request `#8 <https://github.com/open-rdc/icart_mini_gazebo/issues/8>`_ from open-rdc/fix-publish-odom-rate
+  Optimize the update rate of the odometry
+* Optimize the update rate of the odometry
+* Update README.md
+* Merge pull request `#7 <https://github.com/open-rdc/icart_mini_gazebo/issues/7>`_ from open-rdc/change-rate-odom-pub
+  Change the update rate of odometry_publisher
+* Change the update rate of odometry_publisher
+* Update README.md
+* Merge pull request `#6 <https://github.com/open-rdc/icart_mini_gazebo/issues/6>`_ from open-rdc/add-combine-odom-launch
+  Add the process to start up the combine_dr_measurements node
+* Added the launch node of the combine_dr_measurements
+* Merge pull request `#5 <https://github.com/open-rdc/icart_mini_gazebo/issues/5>`_ from open-rdc/imp-hw-sim
+  Implemented the derived class from RobotHWSim
+* Description of building settings
+* Added an configuration file for the pluginlib
+* Implemented the derived class from RobotHWSim
+* Added dependency packages
+* Merge pull request `#4 <https://github.com/open-rdc/icart_mini_gazebo/issues/4>`_ from open-rdc/change-default-world
+  Abolish the willowgarage map because robot description is not updated
+* Abolish this world because robot description is not updated
+* Modified to use the empty world
+* Fixed runtime error
+* Merge pull request `#3 <https://github.com/open-rdc/icart_mini_gazebo/issues/3>`_ from open-rdc/change-project-name
+  Change the project name from t_frog_gazebo to icart_mini_gazebo
+* Change the file name of the .world
+* Fixed an error of reference to the package
+* Fixed build error
+* Update package.xml
+* Update README.md
+* Merge pull request `#2 <https://github.com/open-rdc/icart_mini_gazebo/issues/2>`_ from open-rdc/add-world-file
+  Change a map file from the empty world to the willowgarage world
+* Change a map file from the empty world to the willowgarage world
+* Added the new world file
+* Delete unnecessary include files
+* Fixed include file name
+* Added launch method of 2DNavigation
+* Move directory
+* Move directory
+* Fixed typo
+* Delete unnecessary packages
+* Merge pull request `#1 <https://github.com/open-rdc/icart_mini_gazebo/issues/1>`_ from open-rdc/fix-namespace
+  Fixed a namespace of the t_frog model
+* Fixed a namespace of the t_frog model
+* Added parameters of the t_frog_control
+* Change the controller type
+* Fix for using t_frog_control
+* Provisional implement the model of T-frog
+* Fix a problem that laser settings are not reflected
+* Initial commit
+* Initial commit
+* Contributors: Daiki Maekawa, DaikiMaekawa

icart_mini_gazebo/CMakeLists.txt

@@ -0,0 +1,51 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(icart_mini_gazebo)
+
+find_package(catkin REQUIRED COMPONENTS
+    roscpp
+    gazebo_ros
+    controller_manager
+    hardware_interface
+    transmission_interface
+    pluginlib
+    joint_limits_interface
+    urdf
+    gazebo_ros_control
+    #safety_interface
+)
+
+find_package(gazebo)
+
+catkin_package(
+    DEPENDS
+    CATKIN_DEPENDS
+      roscpp
+      gazebo_ros
+      controller_manager
+      pluginlib
+      transmission_interface
+      gazebo_ros_control
+      #safety_interface
+    INCLUDE_DIRS
+    LIBRARIES gazebo
+)
+
+link_directories(${GAZEBO_LIBRARY_DIRS})
+include_directories(
+    ${Boost_INCLUDE_DIR}
+    ${catkin_INCLUDE_DIRS}
+    ${GAZEBO_INCLUDE_DIRS}
+)
+
+add_library(icart_mini_hw_sim src/icart_mini_hw_sim.cpp)
+target_link_libraries(icart_mini_hw_sim 
+    ${catkin_LIBRARIES} 
+    ${GAZEBO_LIBRARIES}
+)
+
+install(FILES icart_mini_hw_sim_plugins.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
+install(TARGETS icart_mini_hw_sim
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
+

icart_mini_gazebo/icart_mini_hw_sim_plugins.xml

@@ -0,0 +1,10 @@
+<library path="lib/libicart_mini_hw_sim">
+    <class
+        name="icart_mini_gazebo/ICartMiniHWSim"
+        type="icart_mini_gazebo::ICartMiniHWSim"
+        base_class_type="gazebo_ros_control::RobotHWSim">
+        <description>
+            TODO
+        </description>
+    </class>
+</library>

icart_mini_gazebo/launch/icart_mini.launch

@@ -0,0 +1,38 @@
+<launch>
+
+  <!-- these are the arguments you can pass this launch file, for example paused:=true -->
+  <arg name="paused" default="false"/>
+  <arg name="use_sim_time" default="true"/>
+  <arg name="gui" default="true"/>
+  <arg name="headless" default="false"/>
+  <arg name="debug" default="false"/>
+
+  <!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="world_name" value="$(find icart_mini_gazebo)/worlds/empty.world"/>
+    <arg name="debug" value="$(arg debug)" />
+    <arg name="gui" value="$(arg gui)" />
+    <arg name="paused" value="$(arg paused)"/>
+    <arg name="use_sim_time" value="$(arg use_sim_time)"/>
+    <arg name="headless" value="$(arg headless)"/>
+  </include>
+
+  <!-- Load the URDF into the ROS Parameter Server -->
+  <param name="robot_description"
+         command="$(find xacro)/xacro.py '$(find icart_mini_description)/urdf/icart_mini.xacro'" />
+
+  <!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
+  <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
+        args="-urdf -model icart_mini -param robot_description"/>
+
+  <node name="combine_dr_measurements" pkg="combine_dr_measurements" type="odometry_publisher">
+    <param name="max_update_rate" value="20"/>
+    <remap from="odom" to="/icart_mini/odom"/>
+    <remap from="imu" to="/icart_mini/imu"/>
+  </node>
+
+  <!-- ros_control icart_mini launch file -->
+  <include file="$(find icart_mini_control)/launch/icart_mini_control.launch" />
+
+</launch>
+

icart_mini_gazebo/package.xml

@@ -0,0 +1,41 @@
+<?xml version="1.0"?>
+<package>
+  <name>icart_mini_gazebo</name>
+  <version>0.0.1</version>
+  <description>The icart_mini_gazebo package</description>
+
+  <maintainer email="[email protected]">Daiki Maekawa</maintainer>
+
+  <license>BSD</license>
+
+  <author email="[email protected]">Daiki Maekawa</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <!--run_depend>icart_mini_description</run_depend-->
+  <!--run_depend>icart_mini_control</run_depend-->
+  <run_depend>gazebo_ros</run_depend>
+  <run_depend>pluginlib</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>controller_manager</run_depend>
+  <run_depend>transmission_interface</run_depend>
+  <run_depend>gazebo_ros_control</run_depend>
+  <run_depend>hardware_interface</run_depend>
+  <run_depend>joint_limits_interface</run_depend>
+  <run_depend>urdf</run_depend>
+  <run_depend>robot_state_publisher</run_depend>
+  <run_depend>gazebo_plugins</run_depend>
+
+  <build_depend>gazebo_ros</build_depend>
+  <build_depend>pluginlib</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>controller_manager</build_depend>
+  <build_depend>transmission_interface</build_depend>
+  <build_depend>gazebo_ros_control</build_depend>
+  <build_depend>hardware_interface</build_depend>
+  <build_depend>joint_limits_interface</build_depend>
+  <build_depend>urdf</build_depend>
+
+  <export>
+    <gazebo_ros_control plugin="${prefix}/icart_mini_hw_sim_plugins.xml"/>
+  </export>
+</package>

icart_mini_gazebo/src/icart_mini_hw_sim.cpp

@@ -0,0 +1,111 @@
+#include <hardware_interface/joint_command_interface.h>
+#include <hardware_interface/robot_hw.h>
+
+#include <gazebo/common/common.hh>
+#include <gazebo/physics/physics.hh>
+#include <gazebo/gazebo.hh>
+
+#include <ros/ros.h>
+#include <angles/angles.h>
+#include <pluginlib/class_list_macros.h>
+#include <std_msgs/Bool.h>
+
+#include <gazebo_ros_control/robot_hw_sim.h>
+
+#include <urdf/model.h>
+
+//#include <safety_interface/safety_interface.h>
+
+namespace icart_mini_gazebo
+{
+
+    class ICartMiniHWSim : public gazebo_ros_control::RobotHWSim
+    {
+    private:
+        static const double max_drive_joint_torque_ = 20.0;
+
+        double cmd_[2];
+        double pos_[2];
+        double vel_[2];
+        double eff_[2];
+
+        gazebo::physics::JointPtr joint_[2];
+
+        hardware_interface::JointStateInterface js_interface_;
+        hardware_interface::VelocityJointInterface vj_interface_;
+        //safety_interface::SafetyInterface safety_interface_;
+
+    public:
+        bool initSim(const std::string& robot_namespace, ros::NodeHandle model_nh, gazebo::physics::ModelPtr parent_model,
+            const urdf::Model* const urdf_model, std::vector<transmission_interface::TransmissionInfo> transmissions)
+        {
+            pos_[0] = 0.0; pos_[1] = 0.0;
+            vel_[0] = 0.0; vel_[1] = 0.0;
+            eff_[0] = 0.0; eff_[1] = 0.0;
+            cmd_[0] = 0.0; cmd_[1] = 0.0;
+
+            std::string joint_namespace = robot_namespace.substr(1); //remove leading slash
+
+            std::cout << "joint_namespace = " << joint_namespace << std::endl;
+
+            joint_[0] = parent_model->GetJoint("right_wheel_hinge");
+            joint_[1] = parent_model->GetJoint("left_wheel_hinge");
+
+            //joint_[0] = parent_model->GetJoint(joint_namespace + "/right_wheel_hinge");
+            //joint_[1] = parent_model->GetJoint(joint_namespace + "/left_wheel_hinge");
+
+            js_interface_.registerHandle(
+                //hardware_interface::JointStateHandle(joint_namespace + "/right_wheel_hinge", &pos_[0], &vel_[0], &eff_[0]));
+                hardware_interface::JointStateHandle("right_wheel_hinge", &pos_[0], &vel_[0], &eff_[0]));
+            js_interface_.registerHandle(
+                //hardware_interface::JointStateHandle(joint_namespace + "/left_wheel_hinge", &pos_[1], &vel_[1], &eff_[1]));
+                hardware_interface::JointStateHandle("left_wheel_hinge", &pos_[1], &vel_[1], &eff_[1]));
+
+            vj_interface_.registerHandle(
+                //hardware_interface::JointHandle(js_interface_.getHandle(joint_namespace + "/right_wheel_hinge"), &cmd_[0]));
+                hardware_interface::JointHandle(js_interface_.getHandle("right_wheel_hinge"), &cmd_[0]));
+
+            vj_interface_.registerHandle(
+                //hardware_interface::JointHandle(js_interface_.getHandle(joint_namespace + "/right_wheel_hinge"), &cmd_[1]));
+                hardware_interface::JointHandle(js_interface_.getHandle("left_wheel_hinge"), &cmd_[1]));
+
+            registerInterface(&js_interface_);
+            registerInterface(&vj_interface_);
+
+            //registerInterface(&safety_interface_);
+
+            return true;
+        }
+
+        void readSim(ros::Time time, ros::Duration period){
+            for(int i=0; i < 2; i++){
+                pos_[i] += angles::shortest_angular_distance(pos_[i], joint_[i]->GetAngle(0).Radian());
+                vel_[i] = joint_[i]->GetVelocity(0);
+                eff_[i] = joint_[i]->GetForce((unsigned int)(0));
+            }
+        }
+
+        void writeSim(ros::Time time, ros::Duration period){
+            for(int i=0; i < 2; i++){
+                joint_[i]->SetVelocity(0, cmd_[i]);
+                joint_[i]->SetMaxForce(0, max_drive_joint_torque_);
+            }
+
+            /*
+            if(safety_interface_.get_state() == safety_interface::safety_state::OK){
+                for(int i=0; i < 2; i++){
+                    joint_[i]->SetVelocity(0, cmd_[i]);
+                    joint_[i]->SetMaxForce(0, max_drive_joint_torque_);
+                }
+            }else{
+                for(int i=0; i < 2; i++) joint_[i]->SetVelocity(0, 0);
+            }
+            */
+        }
+    };
+
+    typedef boost::shared_ptr<ICartMiniHWSim> ICartMiniHWSimPtr;
+}
+
+PLUGINLIB_EXPORT_CLASS(icart_mini_gazebo::ICartMiniHWSim, gazebo_ros_control::RobotHWSim)
+

icart_mini_gazebo/worlds/empty.world

@@ -0,0 +1,24 @@
+<?xml version="1.0" ?>
+<sdf version="1.4">
+
+  <world name="default">
+    <include>
+      <uri>model://ground_plane</uri>
+    </include>
+
+    <!-- Global light source -->
+    <include>
+      <uri>model://sun</uri>
+    </include>
+
+    <!-- Focus camera on tall pendulum -->
+    <gui fullscreen='0'>
+      <camera name='user_camera'>
+        <pose>4.927360 -4.376610 3.740080 0.000000 0.275643 2.356190</pose>
+        <view_controller>orbit</view_controller>
+      </camera>
+    </gui>
+
+  </world>
+</sdf>
+