Skip to content

Commit

Permalink
Merge pull request #77 from tongtybj/truck_pos_pub
Browse files Browse the repository at this point in the history 
Add the publisher about "true" pose of heliport for the evaluation of…
  • Loading branch information
@k-okada
k-okada authored Jun 13, 2016
2 parents db20114 + 9c97906 commit 2e52ab1
Show file tree
Hide file tree
Showing 7 changed files with 293 additions and 25 deletions.
14 changes: 0 additions & 14 deletions jsk_mbzirc_common/gazebo_model/world/mbzirc_arena_task_1.world
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,21 +31,7 @@
</include>

<include>
<!-- <pose>0 0 0 0 -0 0</pose> -->
<uri>model://arena</uri>
</include>

<include>
<pose>-12.9517 13.7239 0.0 0 0 -0.81434</pose>
<uri>model://truck</uri>
<plugin name="mbzirc_gazebo_truck_plugin" filename="libmbzirc_gazebo_truck_plugin.so"/>
</include>

<gui fullscreen='0'>
<camera name='user_camera'>
<pose>159.908 -1.39949 83.4967 0 0.559643 -3.11099</pose>
<view_controller>orbit</view_controller>
</camera>
</gui>
</world>
</sdf>
21 changes: 21 additions & 0 deletions jsk_mbzirc_common/launch/mbzirc_arena_1.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,11 +4,32 @@
<arg name="gui" default="true" />
<arg name="headless" default="false"/>


<include file="$(find jsk_mbzirc_common)/launch/mbzirc_arena.launch" >
<arg name="paused" default="$(arg paused)"/>
<arg name="debug" default="$(arg debug)"/>
<arg name="gui" default="$(arg gui)" />
<arg name="world_name" default="$(find jsk_mbzirc_common)/gazebo_model/world/mbzirc_arena_task_1.world"/>
<arg name="headless" default="$(arg headless)"/>
</include>

<param name="truck/robot_description" command="$(find xacro)/xacro '$(find jsk_mbzirc_common)/urdf/truck.urdf.xacro'" />

<group ns="truck/ground_truth">
<node name="spawn_robot" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
args=" -urdf -model truck -param /truck/robot_description" />

<node pkg="fake_localization" type="fake_localization" name="fake_localization" output="screen" >
<remap from="base_pose_ground_truth" to="/truck/ground_truth/odom"/>
<param name="global_frame_id " value="/world"/>
<param name="base_frame_id" value="/truck/ground_truth/base_link"/>
<param name="odom_frame_id" value="/truck/ground_truth/base_link"/>
</node>

<node pkg="robot_state_publisher" type="robot_state_publisher" name="trcuk_state_publisher" output="screen" >
<param name="tf_prefix" type="string" value="truck/ground_truth" />
<param name="publish_frequency" type="double" value="50.0" />
</node>
</group>

</launch>
3 changes: 3 additions & 0 deletions jsk_mbzirc_common/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,9 @@
<run_depend>gazebo_plugins</run_depend>
<run_depend>gazebo_ros</run_depend>

<run_depend>robot_state_publisher</run_depend>
<run_depend>fake_localization</run_depend>

<test_depend>roslint</test_depend>
<test_depend>rostest</test_depend>
<test_depend>roslaunch</test_depend>
Expand Down
18 changes: 11 additions & 7 deletions jsk_mbzirc_common/src/mbzirc_gazebo_truck_plugin.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -86,8 +86,8 @@ void GazeboTruck::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
}

node_handle_ = new ros::NodeHandle(namespace_);
pub_score_ = node_handle_->advertise<std_msgs::String>("score", 1, true); // set latch true
pub_time_ = node_handle_->advertise<std_msgs::String>("remaining_time", 1);
pub_score_ = node_handle_->advertise<std_msgs::String>("/score", 1, true); // set latch true
pub_time_ = node_handle_->advertise<std_msgs::String>("/remaining_time", 1);

update_connection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboTruck::Update, this));
}
Expand Down Expand Up @@ -161,7 +161,7 @@ void GazeboTruck::Update()
y = -CIRCLE_RADIUS * sin(l/l2*(2*M_PI-theta*2)+theta);
yaw = -(l/l2*(2*M_PI-theta*2)+theta)-M_PI/2;
}
model_->SetLinkWorldPose(math::Pose(x, y, 0.595, 0, 0, yaw), link_);
model_->SetLinkWorldPose(math::Pose(x, y, 0, 0, 0, yaw), link_);
last_time_ = world_->GetSimTime();

// check score
Expand All @@ -172,8 +172,12 @@ void GazeboTruck::Update()

double distAbove;
std::string entityName;
math::Box box = model_->GetLink("heliport")->GetCollisionBoundingBox();
math::Vector3 start = model_->GetLink("heliport")->GetWorldPose().pos;
math::Box box = model_->GetLink("base_link")->GetCollision("base_link_collision_heliport")->GetBoundingBox();
math::Vector3 start = model_->GetWorldPose().pos;

start.x += (-0.5) * cos(yaw);
start.y += (-0.5) * sin(yaw);

math::Vector3 end = start;
start.z = box.max.z + 0.00001;
end.z += 1000;
Expand All @@ -187,11 +191,11 @@ void GazeboTruck::Update()
ss << 20*60 - current_time.Double();
msg_time.data = "remain time:" + ss.str();
pub_time_.publish(msg_time);
if ( entityName != "" && distAbove < 1.0 )
if ( entityName != "" && distAbove < 1.0 )
{
std_msgs::String msg_score, msg_time;
msg_score.data = "Mission Completed";
ROS_INFO_STREAM("Remaining time is " << msg_time.data << "[sec], Score is " << msg_score.data);
ROS_INFO_STREAM("Remaining time is " << ss.str() << "[sec], Score is " << msg_score.data);
pub_score_.publish(msg_score);
terminated_ = true;
}
Expand Down
254 changes: 254 additions & 0 deletions jsk_mbzirc_common/urdf/truck.urdf.xacro
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,254 @@
<?xml version="1.0"?>

<robot name="truck" xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:property name="pi" value="3.1415926535897931" />

<!-- body -->
<link name="base_link">
<inertial>
<mass value="700" /> <!-- http://trucktorack.blog.fc2.com/blog-entry-252.html -->
<origin xyz="0 0 0.595" rpy="0 0 0" />
<inertia ixx="200.0" ixy="0.0" ixz="0.0" iyy="400.0" iyz="0.0" izz="500.0" />
</inertial>

<visual>
<origin xyz="0 0 0.595" rpy="0 0 0" />
<geometry>
<mesh filename="package://jsk_mbzirc_common/gazebo_model/models/truck/meshes/truck.dae" scale=".001 .001 .001"/>
</geometry>
<material name="color"> <!-- blue -->
<color rgba="0 0 0 1"/>
</material>
</visual>

<collision>
<origin xyz="0 0 0.595" rpy="0 0 0" />
<geometry>
<mesh filename="package://jsk_mbzirc_common/gazebo_model/models/truck/meshes/truck.dae" scale=".001 .001 .001"/>
</geometry>
</collision>
</link>

<!-- tire -->
<joint name="body_to_front_left_tire" type="fixed">
<parent link="base_link"/>
<child link="front_left_tire"/>
<origin rpy="${pi/2} 0 0" xyz="0.8 0.625 0.295"/>
</joint>

<link name="front_left_tire">
<inertial>
<mass value="20" />
<origin xyz="0 0 0" rpy="0 0 0" />
<inertia ixx="0.78" ixy="0.0" ixz="0.0" iyy="0.78" iyz="0.0" izz="0.87" />
</inertial>

<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder radius=".295" length=".23"/>
</geometry>
<material name="color">
<color rgba="0 0 0 1"/>
</material>
</visual>

<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder radius=".295" length=".23"/>
</geometry>
</collision>
</link>

<joint name="body_to_front_right_tire" type="fixed">
<parent link="base_link"/>
<child link="front_right_tire"/>
<origin rpy="${pi/2} 0 0" xyz="0.8 -0.625 0.295"/>
</joint>

<link name="front_right_tire">
<inertial>
<mass value="20" />
<origin xyz="0 0 0" rpy="0 0 0" />
<inertia ixx="0.78" ixy="0.0" ixz="0.0" iyy="0.78" iyz="0.0" izz="0.87" />
</inertial>

<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder radius=".295" length=".23"/>
</geometry>
<material name="color">
<color rgba="0 0 0 1"/>
</material>
</visual>

<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder radius=".295" length=".23"/>
</geometry>
</collision>
</link>

<joint name="body_to_rear_left_tire" type="fixed">
<parent link="base_link"/>
<child link="rear_left_tire"/>
<origin rpy="${pi/2} 0 0" xyz="-0.8 0.625 0.295"/>
</joint>

<link name="rear_left_tire">
<inertial>
<mass value="20" />
<origin xyz="0 0 0" rpy="0 0 0" />
<inertia ixx="0.78" ixy="0.0" ixz="0.0" iyy="0.78" iyz="0.0" izz="0.87" />
</inertial>

<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder radius=".295" length=".23"/>
</geometry>
<material name="color">
<color rgba="0 0 0 1"/>
</material>
</visual>

<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder radius=".295" length=".23"/>
</geometry>
</collision>
</link>

<joint name="body_to_rear_right_tire" type="fixed">
<parent link="base_link"/>
<child link="rear_right_tire"/>
<origin rpy="${pi/2} 0 0" xyz="-0.8 -0.625 0.295"/>
</joint>

<link name="rear_right_tire">
<inertial>
<mass value="20" />
<origin xyz="0 0 0" rpy="0 0 0" />
<inertia ixx="0.78" ixy="0.0" ixz="0.0" iyy="0.78" iyz="0.0" izz="0.87" />
</inertial>

<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder radius=".295" length=".23"/>
</geometry>
<material name="color">
<color rgba="0 0 0 1"/>
</material>
</visual>

<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder radius=".295" length=".23"/>
</geometry>
</collision>
</link>

<joint name="body_to_heliport" type="fixed">
<parent link="base_link"/>
<child link="heliport"/>
<origin rpy="0 0 0" xyz="-0.5 0 0.995"/>
</joint>

<link name="heliport">
<inertial>
<mass value="5" />
<origin xyz="0 0 0" rpy="0 0 0" />
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="2.0" />
</inertial>

<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://jsk_mbzirc_common/gazebo_model/models/truck/meshes/heliport.dae"/>
</geometry>
</visual>

<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://jsk_mbzirc_common/gazebo_model/models/truck/meshes/heliport.dae"/>
</geometry>
</collision>
</link>

<!-- gazebo -->
<gazebo>
<!-- p3d plugin to publish odom -->
<plugin name="ground_truth" filename="libgazebo_ros_p3d.so">
<tf_prefix>truck/ground_truth</tf_prefix>
<frameName>world</frameName>
<bodyName>base_link</bodyName>
<topicName>odom</topicName>
<updateRate>50.0</updateRate>
</plugin>
<!-- truck trajectory + task1 rule -->
<plugin name="mbzirc_gazebo_truck_plugin" filename="libmbzirc_gazebo_truck_plugin.so">
<bodyName>base_link</bodyName>
</plugin>
</gazebo>

<!-- physical property of each link in gazebo -->
<gazebo reference="base_link">
<mu1>100</mu1>
<mu2>50</mu2>
<kp>1e+13</kp>
<kd>1</kd>
<gravity>0</gravity>
</gazebo>

<gazebo reference="heliport">
<mu1>100</mu1>
<mu2>50</mu2>
<kp>1e+13</kp>
<kd>1</kd>
<gravity>0</gravity>
</gazebo>

<gazebo reference="front_left_tire">
<mu1>100</mu1>
<mu2>50</mu2>
<kp>1e+13</kp>
<kd>1</kd>
<material>Gazebo/Black</material>
<gravity>0</gravity>
</gazebo>

<gazebo reference="front_right_tire">
<mu1>100</mu1>
<mu2>50</mu2>
<kp>1e+13</kp>
<kd>1</kd>
<material>Gazebo/Black</material>
<gravity>0</gravity>
</gazebo>

<gazebo reference="rear_left_tire">
<mu1>100</mu1>
<mu2>50</mu2>
<kp>1e+13</kp>
<kd>1</kd>
<material>Gazebo/Black</material>
<gravity>0</gravity>
</gazebo>

<gazebo reference="rear_right_tire">
<mu1>100</mu1>
<mu2>50</mu2>
<kp>1e+13</kp>
<kd>1</kd>
<material>Gazebo/Black</material>
<gravity>0</gravity>
</gazebo>

</robot>
6 changes: 3 additions & 3 deletions jsk_mbzirc_tasks/config/task1.rviz
View file
Original file line number Diff line number Diff line change
Expand Up @@ -145,12 +145,12 @@ Visualization Manager:
Topic: /remaining_time_overlay
Value: true
font: DejaVu Sans Mono
height: 100
height: 50
left: 10
line width: 2
text size: 64
text size: 32
top: 10
width: 370
width: 500
Enabled: true
Global Options:
Background Color: 48; 48; 48
Expand Down
2 changes: 1 addition & 1 deletion jsk_mbzirc_tasks/script/task_1_cheat.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,7 @@
pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)

# for for 1 sec
while rospy.get_time() < 2:
while rospy.get_time() < 2.5:
rospy.sleep(0.01)
rospy.loginfo("start program %f" % rospy.get_time())
msg = Twist()
Expand Down

0 comments on commit 2e52ab1

Please sign in to comment.