Skip to content

Commit 281444e

Browse files
DaikiMaekawaDaikiMaekawa
committed
Merge pull request #53 from open-rdc/add-gazebo-pkg
Moved from the icart_mini_gazebo package
2 parents fe41667 + 3c2894f commit 281444e

File tree

7 files changed

+336
-0
lines changed

7 files changed

+336
-0
lines changed

Diff for: ‎icart_mini_gazebo/CHANGELOG.rst

+61
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,61 @@
1+
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
2+
Changelog for package icart_mini_gazebo
3+
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
4+
5+
Forthcoming
6+
-----------
7+
* Merge pull request `#9 <https://github.com/open-rdc/icart_mini_gazebo/issues/9>`_ from open-rdc/fix-depend-pkg
8+
Add the dependency packages
9+
* Add the dependency package
10+
* Fixed the rosdep error
11+
* Merge pull request `#8 <https://github.com/open-rdc/icart_mini_gazebo/issues/8>`_ from open-rdc/fix-publish-odom-rate
12+
Optimize the update rate of the odometry
13+
* Optimize the update rate of the odometry
14+
* Update README.md
15+
* Merge pull request `#7 <https://github.com/open-rdc/icart_mini_gazebo/issues/7>`_ from open-rdc/change-rate-odom-pub
16+
Change the update rate of odometry_publisher
17+
* Change the update rate of odometry_publisher
18+
* Update README.md
19+
* Merge pull request `#6 <https://github.com/open-rdc/icart_mini_gazebo/issues/6>`_ from open-rdc/add-combine-odom-launch
20+
Add the process to start up the combine_dr_measurements node
21+
* Added the launch node of the combine_dr_measurements
22+
* Merge pull request `#5 <https://github.com/open-rdc/icart_mini_gazebo/issues/5>`_ from open-rdc/imp-hw-sim
23+
Implemented the derived class from RobotHWSim
24+
* Description of building settings
25+
* Added an configuration file for the pluginlib
26+
* Implemented the derived class from RobotHWSim
27+
* Added dependency packages
28+
* Merge pull request `#4 <https://github.com/open-rdc/icart_mini_gazebo/issues/4>`_ from open-rdc/change-default-world
29+
Abolish the willowgarage map because robot description is not updated
30+
* Abolish this world because robot description is not updated
31+
* Modified to use the empty world
32+
* Fixed runtime error
33+
* Merge pull request `#3 <https://github.com/open-rdc/icart_mini_gazebo/issues/3>`_ from open-rdc/change-project-name
34+
Change the project name from t_frog_gazebo to icart_mini_gazebo
35+
* Change the file name of the .world
36+
* Fixed an error of reference to the package
37+
* Fixed build error
38+
* Update package.xml
39+
* Update README.md
40+
* Merge pull request `#2 <https://github.com/open-rdc/icart_mini_gazebo/issues/2>`_ from open-rdc/add-world-file
41+
Change a map file from the empty world to the willowgarage world
42+
* Change a map file from the empty world to the willowgarage world
43+
* Added the new world file
44+
* Delete unnecessary include files
45+
* Fixed include file name
46+
* Added launch method of 2DNavigation
47+
* Move directory
48+
* Move directory
49+
* Fixed typo
50+
* Delete unnecessary packages
51+
* Merge pull request `#1 <https://github.com/open-rdc/icart_mini_gazebo/issues/1>`_ from open-rdc/fix-namespace
52+
Fixed a namespace of the t_frog model
53+
* Fixed a namespace of the t_frog model
54+
* Added parameters of the t_frog_control
55+
* Change the controller type
56+
* Fix for using t_frog_control
57+
* Provisional implement the model of T-frog
58+
* Fix a problem that laser settings are not reflected
59+
* Initial commit
60+
* Initial commit
61+
* Contributors: Daiki Maekawa, DaikiMaekawa

Diff for: ‎icart_mini_gazebo/CMakeLists.txt

+51
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,51 @@
1+
cmake_minimum_required(VERSION 2.8.3)
2+
project(icart_mini_gazebo)
3+
4+
find_package(catkin REQUIRED COMPONENTS
5+
roscpp
6+
gazebo_ros
7+
controller_manager
8+
hardware_interface
9+
transmission_interface
10+
pluginlib
11+
joint_limits_interface
12+
urdf
13+
gazebo_ros_control
14+
#safety_interface
15+
)
16+
17+
find_package(gazebo)
18+
19+
catkin_package(
20+
DEPENDS
21+
CATKIN_DEPENDS
22+
roscpp
23+
gazebo_ros
24+
controller_manager
25+
pluginlib
26+
transmission_interface
27+
gazebo_ros_control
28+
#safety_interface
29+
INCLUDE_DIRS
30+
LIBRARIES gazebo
31+
)
32+
33+
link_directories(${GAZEBO_LIBRARY_DIRS})
34+
include_directories(
35+
${Boost_INCLUDE_DIR}
36+
${catkin_INCLUDE_DIRS}
37+
${GAZEBO_INCLUDE_DIRS}
38+
)
39+
40+
add_library(icart_mini_hw_sim src/icart_mini_hw_sim.cpp)
41+
target_link_libraries(icart_mini_hw_sim
42+
${catkin_LIBRARIES}
43+
${GAZEBO_LIBRARIES}
44+
)
45+
46+
install(FILES icart_mini_hw_sim_plugins.xml
47+
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
48+
49+
install(TARGETS icart_mini_hw_sim
50+
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
51+

Diff for: ‎icart_mini_gazebo/icart_mini_hw_sim_plugins.xml

+10
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,10 @@
1+
<library path="lib/libicart_mini_hw_sim">
2+
<class
3+
name="icart_mini_gazebo/ICartMiniHWSim"
4+
type="icart_mini_gazebo::ICartMiniHWSim"
5+
base_class_type="gazebo_ros_control::RobotHWSim">
6+
<description>
7+
TODO
8+
</description>
9+
</class>
10+
</library>

Diff for: ‎icart_mini_gazebo/launch/icart_mini.launch

+38
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,38 @@
1+
<launch>
2+
3+
<!-- these are the arguments you can pass this launch file, for example paused:=true -->
4+
<arg name="paused" default="false"/>
5+
<arg name="use_sim_time" default="true"/>
6+
<arg name="gui" default="true"/>
7+
<arg name="headless" default="false"/>
8+
<arg name="debug" default="false"/>
9+
10+
<!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
11+
<include file="$(find gazebo_ros)/launch/empty_world.launch">
12+
<arg name="world_name" value="$(find icart_mini_gazebo)/worlds/empty.world"/>
13+
<arg name="debug" value="$(arg debug)" />
14+
<arg name="gui" value="$(arg gui)" />
15+
<arg name="paused" value="$(arg paused)"/>
16+
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
17+
<arg name="headless" value="$(arg headless)"/>
18+
</include>
19+
20+
<!-- Load the URDF into the ROS Parameter Server -->
21+
<param name="robot_description"
22+
command="$(find xacro)/xacro.py '$(find icart_mini_description)/urdf/icart_mini.xacro'" />
23+
24+
<!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
25+
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
26+
args="-urdf -model icart_mini -param robot_description"/>
27+
28+
<node name="combine_dr_measurements" pkg="combine_dr_measurements" type="odometry_publisher">
29+
<param name="max_update_rate" value="20"/>
30+
<remap from="odom" to="/icart_mini/odom"/>
31+
<remap from="imu" to="/icart_mini/imu"/>
32+
</node>
33+
34+
<!-- ros_control icart_mini launch file -->
35+
<include file="$(find icart_mini_control)/launch/icart_mini_control.launch" />
36+
37+
</launch>
38+

Diff for: ‎icart_mini_gazebo/package.xml

+41
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,41 @@
1+
<?xml version="1.0"?>
2+
<package>
3+
<name>icart_mini_gazebo</name>
4+
<version>0.0.1</version>
5+
<description>The icart_mini_gazebo package</description>
6+
7+
<maintainer email="[email protected]">Daiki Maekawa</maintainer>
8+
9+
<license>BSD</license>
10+
11+
<author email="[email protected]">Daiki Maekawa</author>
12+
13+
<buildtool_depend>catkin</buildtool_depend>
14+
<!--run_depend>icart_mini_description</run_depend-->
15+
<!--run_depend>icart_mini_control</run_depend-->
16+
<run_depend>gazebo_ros</run_depend>
17+
<run_depend>pluginlib</run_depend>
18+
<run_depend>roscpp</run_depend>
19+
<run_depend>controller_manager</run_depend>
20+
<run_depend>transmission_interface</run_depend>
21+
<run_depend>gazebo_ros_control</run_depend>
22+
<run_depend>hardware_interface</run_depend>
23+
<run_depend>joint_limits_interface</run_depend>
24+
<run_depend>urdf</run_depend>
25+
<run_depend>robot_state_publisher</run_depend>
26+
<run_depend>gazebo_plugins</run_depend>
27+
28+
<build_depend>gazebo_ros</build_depend>
29+
<build_depend>pluginlib</build_depend>
30+
<build_depend>roscpp</build_depend>
31+
<build_depend>controller_manager</build_depend>
32+
<build_depend>transmission_interface</build_depend>
33+
<build_depend>gazebo_ros_control</build_depend>
34+
<build_depend>hardware_interface</build_depend>
35+
<build_depend>joint_limits_interface</build_depend>
36+
<build_depend>urdf</build_depend>
37+
38+
<export>
39+
<gazebo_ros_control plugin="${prefix}/icart_mini_hw_sim_plugins.xml"/>
40+
</export>
41+
</package>

Diff for: ‎icart_mini_gazebo/src/icart_mini_hw_sim.cpp

+111
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,111 @@
1+
#include <hardware_interface/joint_command_interface.h>
2+
#include <hardware_interface/robot_hw.h>
3+
4+
#include <gazebo/common/common.hh>
5+
#include <gazebo/physics/physics.hh>
6+
#include <gazebo/gazebo.hh>
7+
8+
#include <ros/ros.h>
9+
#include <angles/angles.h>
10+
#include <pluginlib/class_list_macros.h>
11+
#include <std_msgs/Bool.h>
12+
13+
#include <gazebo_ros_control/robot_hw_sim.h>
14+
15+
#include <urdf/model.h>
16+
17+
//#include <safety_interface/safety_interface.h>
18+
19+
namespace icart_mini_gazebo
20+
{
21+
22+
class ICartMiniHWSim : public gazebo_ros_control::RobotHWSim
23+
{
24+
private:
25+
static const double max_drive_joint_torque_ = 20.0;
26+
27+
double cmd_[2];
28+
double pos_[2];
29+
double vel_[2];
30+
double eff_[2];
31+
32+
gazebo::physics::JointPtr joint_[2];
33+
34+
hardware_interface::JointStateInterface js_interface_;
35+
hardware_interface::VelocityJointInterface vj_interface_;
36+
//safety_interface::SafetyInterface safety_interface_;
37+
38+
public:
39+
bool initSim(const std::string& robot_namespace, ros::NodeHandle model_nh, gazebo::physics::ModelPtr parent_model,
40+
const urdf::Model* const urdf_model, std::vector<transmission_interface::TransmissionInfo> transmissions)
41+
{
42+
pos_[0] = 0.0; pos_[1] = 0.0;
43+
vel_[0] = 0.0; vel_[1] = 0.0;
44+
eff_[0] = 0.0; eff_[1] = 0.0;
45+
cmd_[0] = 0.0; cmd_[1] = 0.0;
46+
47+
std::string joint_namespace = robot_namespace.substr(1); //remove leading slash
48+
49+
std::cout << "joint_namespace = " << joint_namespace << std::endl;
50+
51+
joint_[0] = parent_model->GetJoint("right_wheel_hinge");
52+
joint_[1] = parent_model->GetJoint("left_wheel_hinge");
53+
54+
//joint_[0] = parent_model->GetJoint(joint_namespace + "/right_wheel_hinge");
55+
//joint_[1] = parent_model->GetJoint(joint_namespace + "/left_wheel_hinge");
56+
57+
js_interface_.registerHandle(
58+
//hardware_interface::JointStateHandle(joint_namespace + "/right_wheel_hinge", &pos_[0], &vel_[0], &eff_[0]));
59+
hardware_interface::JointStateHandle("right_wheel_hinge", &pos_[0], &vel_[0], &eff_[0]));
60+
js_interface_.registerHandle(
61+
//hardware_interface::JointStateHandle(joint_namespace + "/left_wheel_hinge", &pos_[1], &vel_[1], &eff_[1]));
62+
hardware_interface::JointStateHandle("left_wheel_hinge", &pos_[1], &vel_[1], &eff_[1]));
63+
64+
vj_interface_.registerHandle(
65+
//hardware_interface::JointHandle(js_interface_.getHandle(joint_namespace + "/right_wheel_hinge"), &cmd_[0]));
66+
hardware_interface::JointHandle(js_interface_.getHandle("right_wheel_hinge"), &cmd_[0]));
67+
68+
vj_interface_.registerHandle(
69+
//hardware_interface::JointHandle(js_interface_.getHandle(joint_namespace + "/right_wheel_hinge"), &cmd_[1]));
70+
hardware_interface::JointHandle(js_interface_.getHandle("left_wheel_hinge"), &cmd_[1]));
71+
72+
registerInterface(&js_interface_);
73+
registerInterface(&vj_interface_);
74+
75+
//registerInterface(&safety_interface_);
76+
77+
return true;
78+
}
79+
80+
void readSim(ros::Time time, ros::Duration period){
81+
for(int i=0; i < 2; i++){
82+
pos_[i] += angles::shortest_angular_distance(pos_[i], joint_[i]->GetAngle(0).Radian());
83+
vel_[i] = joint_[i]->GetVelocity(0);
84+
eff_[i] = joint_[i]->GetForce((unsigned int)(0));
85+
}
86+
}
87+
88+
void writeSim(ros::Time time, ros::Duration period){
89+
for(int i=0; i < 2; i++){
90+
joint_[i]->SetVelocity(0, cmd_[i]);
91+
joint_[i]->SetMaxForce(0, max_drive_joint_torque_);
92+
}
93+
94+
/*
95+
if(safety_interface_.get_state() == safety_interface::safety_state::OK){
96+
for(int i=0; i < 2; i++){
97+
joint_[i]->SetVelocity(0, cmd_[i]);
98+
joint_[i]->SetMaxForce(0, max_drive_joint_torque_);
99+
}
100+
}else{
101+
for(int i=0; i < 2; i++) joint_[i]->SetVelocity(0, 0);
102+
}
103+
*/
104+
}
105+
};
106+
107+
typedef boost::shared_ptr<ICartMiniHWSim> ICartMiniHWSimPtr;
108+
}
109+
110+
PLUGINLIB_EXPORT_CLASS(icart_mini_gazebo::ICartMiniHWSim, gazebo_ros_control::RobotHWSim)
111+

Diff for: ‎icart_mini_gazebo/worlds/empty.world

+24
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,24 @@
1+
<?xml version="1.0" ?>
2+
<sdf version="1.4">
3+
4+
<world name="default">
5+
<include>
6+
<uri>model://ground_plane</uri>
7+
</include>
8+
9+
<!-- Global light source -->
10+
<include>
11+
<uri>model://sun</uri>
12+
</include>
13+
14+
<!-- Focus camera on tall pendulum -->
15+
<gui fullscreen='0'>
16+
<camera name='user_camera'>
17+
<pose>4.927360 -4.376610 3.740080 0.000000 0.275643 2.356190</pose>
18+
<view_controller>orbit</view_controller>
19+
</camera>
20+
</gui>
21+
22+
</world>
23+
</sdf>
24+

0 commit comments

Comments
 (0)