Merge pull request #11 from WHILL/feature/#8

Follow tf structure to REP105 standard. Close #8.
WHILL · Apr 26, 2019 · 6b9491d · 6b9491d
2 parents 0f71cb8 + 2e651d6
commit 6b9491d
Show file tree

Hide file tree

Showing 4 changed files with 7 additions and 15 deletions.
diff --git a/launch/modelc.launch b/launch/modelc.launch
@@ -2,7 +2,7 @@
     <arg name="model" default="$(find ros_whill)/xacro/modelc.xacro" />
     <arg name="gui" default="true" />
 
-    <param name="robot_description" command="$(find xacro)/xacro $(find ros_whill)/xacro/modelc.xacro"/>
+    <param name="robot_description" command="$(find xacro)/xacro $(arg model)"/>
 
     <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher">
         <remap from="/joint_states" to="/whill/states/jointState" />

diff --git a/src/odom.cpp b/src/odom.cpp
@@ -46,6 +46,8 @@ SOFTWARE.
 #include <limits>
 
 
+const float base_link_height = 0.1325;
+
 
 Odometry::Odometry(){
     pose.x = pose.y = pose.theta = 0.0;
@@ -114,7 +116,7 @@ nav_msgs::Odometry Odometry::getROSOdometry(){
     // position
     odom.pose.pose.position.x = pose.x;
     odom.pose.pose.position.y = pose.y;
-    odom.pose.pose.position.z = 0.0;
+    odom.pose.pose.position.z = base_link_height;
     odom.pose.pose.orientation = odom_quat;
 
     //velocity
@@ -138,7 +140,7 @@ geometry_msgs::TransformStamped Odometry::getROSTransformStamped(){
 
     odom_trans.transform.translation.x = pose.x;
     odom_trans.transform.translation.y = pose.y;
-    odom_trans.transform.translation.z = 0.0;
+    odom_trans.transform.translation.z = base_link_height;
     odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(pose.theta);
 
     return  odom_trans;

diff --git a/src/whill_modelc_publisher.cpp b/src/whill_modelc_publisher.cpp
@@ -385,13 +385,13 @@ int main(int argc, char **argv)
                     geometry_msgs::TransformStamped odom_trans = odom.getROSTransformStamped();
                     odom_trans.header.stamp = currentTime;
                     odom_trans.header.frame_id = "odom";
-                    odom_trans.child_frame_id = "base_footprint";
+                    odom_trans.child_frame_id = "base_link";
                     odom_broadcaster_.sendTransform(odom_trans);
 
                     nav_msgs::Odometry odom_msg = odom.getROSOdometry();
                     odom_msg.header.stamp = currentTime;
                     odom_msg.header.frame_id = "odom";
-                    odom_msg.child_frame_id = "base_footprint";
+                    odom_msg.child_frame_id = "base_link";
                     whill_modelc_odom.publish(odom_msg);
                 }
             }

diff --git a/xacro/modelc.xacro b/xacro/modelc.xacro
@@ -1,20 +1,10 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="whill_modelc" >
 
-    <link name="base_footprint">
-
-    </link>
-
     <link name="base_link">
 
     </link>
 
-    <joint name="base_joint" type="fixed">
-        <parent link="base_footprint" />
-        <child link="base_link" />
-        <origin xyz="0 0 0.1325" rpy="0 0 0"/>
-    </joint>
-
     <link name="base_floor">
         <visual>
             <geometry>