odom pub fixed

amirhosein-vedadi · Aug 26, 2022 · cc59e50 · cc59e50
1 parent aeb9db7
commit cc59e50
Show file tree

Hide file tree

Showing 6 changed files with 63 additions and 14 deletions.
diff --git a/model/SurenaV/surena5.wrl b/model/SurenaV/surena5.wrl
@@ -316,7 +316,7 @@ DEF SURENA5 Humanoid {
 
     DEF Pelvis Joint {
       jointType "free"
-      translation 0 0 0.90264
+      translation 0 0 0.6
       children [
         DEF WaistAccelSensor AccelerationSensor { sensorId 0 }
         DEF camera VisionSensor {sensorId 0}

diff --git a/project/SurenaV-wrl-house.cnoid b/project/SurenaV-wrl-house.cnoid
@@ -95,11 +95,12 @@ items:
           name: "SURENA5"
           plugin: Body
           class: BodyItem
+          is_selected: true
           is_checked: true
           data: 
             file: "../model/SurenaV/surena5.wrl"
             format: CHOREONOID-BODY
-            rootPosition: [ 0, 0, 0.90264 ]
+            rootPosition: [ 0, 0, 0.6 ]
             rootAttitude: [ 
               1, 0, 0, 
               0, 1, 0, 
@@ -127,14 +128,16 @@ items:
               "RLeg_AnkleR_J6": 
                 base_frame: 0
                 offset_frame: 0
+              "LLeg_AnkleR_J6": 
+                base_frame: 0
+                offset_frame: 0
             zmp: [ 0, 0, 0 ]
           children: 
             - 
               id: 6
               name: "SimpleController"
               plugin: Body
               class: SimpleControllerItem
-              is_selected: true
               data: 
                 isNoDelayMode: false
                 controllerOptions: ""
@@ -152,7 +155,8 @@ items:
                   data: 
                     isNoDelayMode: false
                     controllerOptions: "CameraSampleController"
-                    baseDirectory: "Controller directory"
+                    controller: "../../../build/surena_simulation/lib/choreonoid-1.8/simplecontroller/CameraController"
+                    baseDirectory: "None"
                     reloading: false
                     exportSymbols: false
                     isOldTargetVariableMode: false
@@ -170,7 +174,7 @@ items:
                     visualRatio: 0.002
                   - 
                     class: CameraImageVisualizerItem
-                    name: camera
+                    name: camera_Image
 views: 
   - 
     id: 0
@@ -207,7 +211,7 @@ views:
     class: SceneView
     mounted: true
     state: 
-      editMode: false
+      editMode: true
       viewpointOperationMode: thirdPerson
       visible_polygon_elements: [ face ]
       highlighting: false
@@ -248,9 +252,9 @@ views:
           fieldOfView: 0.610865
           near: 0.04
           far: 200
-          eye: [ 10.593, 5.26436, 7.76277 ]
-          direction: [ -0.714011, -0.296547, -0.634231 ]
-          up: [ -0.585722, -0.243265, 0.773144 ]
+          eye: [ 5.99295, -0.210363, 4.64507 ]
+          direction: [ -0.601692, 0.088693, -0.793789 ]
+          up: [ -0.785303, 0.115758, 0.608194 ]
         - 
           camera: [ System, Orthographic ]
           orthoHeight: 20
@@ -265,6 +269,7 @@ views:
     id: 5
     plugin: Body
     class: BodyLinkView
+    mounted: true
     state: 
       showRotationMatrix: false
   - 
@@ -289,14 +294,19 @@ views:
     state: 
       element_type: all
       listingMode: list
+      current_body_item: 5
+      body_items: 
+        - 
+          id: 2
+          selected_links: [ 0 ]
   - 
     id: 8
     name: "Link Position"
     plugin: Body
     class: LinkPositionView
     mounted: true
     state: 
-      coordinate_mode: world
+      coordinate_mode: base
       preferred_coordinate_mode: base
       show_rpy: true
       unique_rpy: false
@@ -374,8 +384,8 @@ Body:
   "BodyMotionEngine": 
     updateJointVelocities: false
   "BodySelectionManager": 
-    current_body_item: 2
-    current_link: "Root"
+    current_body_item: 5
+    current_link: "Pelvis"
   "EditableSceneBody": 
     editableSceneBodies: 
       - 
@@ -435,7 +445,8 @@ viewAreas:
               children: 
                 - 
                   type: pane
-                  views: [ 4 ]
+                  views: [ 4, 5 ]
+                  current: 4
                 - 
                   type: pane
                   views: [ 3, 10 ]
@@ -459,7 +470,7 @@ viewAreas:
                 - 
                   type: pane
                   views: [ 7, 6 ]
-                  current: 6
+                  current: 7
 layoutOfToolBars: 
   rows: 
     - 

diff --git a/trajectory_planner/CMakeLists.txt b/trajectory_planner/CMakeLists.txt
@@ -12,7 +12,9 @@ find_package(catkin REQUIRED COMPONENTS
   roslib
   rospy
   std_msgs
+  geometry_msgs
   std_srvs
+  nav_msgs
   message_generation
   choreonoid
 )
@@ -92,6 +94,8 @@ add_service_files(
 generate_messages(
   DEPENDENCIES
   std_msgs
+  nav_msgs
+  geometry_msgs
   std_srvs
 )
 

diff --git a/trajectory_planner/include/trajectory_planner/Robot.h b/trajectory_planner/include/trajectory_planner/Robot.h
@@ -6,6 +6,7 @@
 #include "trajectory_planner/JntAngs.h"
 #include "trajectory_planner/Trajectory.h"
 #include "trajectory_planner/GeneralTraj.h"
+#include "nav_msgs/Odometry.h"
 
 #include "DCM.h"
 #include "Link.h"
@@ -45,6 +46,8 @@ class Robot{
         int findTrajIndex(vector<int> arr, int n, int K);
 
         void distributeFT(Vector3d zmp_y, Vector3d r_foot_y,Vector3d l_foot_y, Vector3d &r_wrench, Vector3d &l_wrench);
+
+        void baseOdomPublisher(Vector3d base_pos, Vector3d base_vel, Quaterniond base_quat);
     private:
 
         double thigh_;
@@ -120,6 +123,9 @@ class Robot{
         ros::ServiceServer trajGenServer_;
         ros::ServiceServer generalTrajServer_;
         ros::ServiceServer resetTrajServer_;
+
+        ros::Publisher baseOdomPub_;
+
         bool isTrajAvailable_;
         bool useController_;
 

diff --git a/trajectory_planner/package.xml b/trajectory_planner/package.xml
@@ -52,12 +52,16 @@
   <build_depend>roscpp</build_depend>
   <build_depend>rospy</build_depend>
   <build_depend>std_msgs</build_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
   <build_export_depend>roscpp</build_export_depend>
   <build_export_depend>rospy</build_export_depend>
   <build_export_depend>std_msgs</build_export_depend>
   <exec_depend>roscpp</exec_depend>
   <exec_depend>rospy</exec_depend>
   <exec_depend>std_msgs</exec_depend>
+  <exec_depend>nav_msgs</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->

diff --git a/trajectory_planner/src/Robot.cpp b/trajectory_planner/src/Robot.cpp
@@ -25,6 +25,7 @@ Robot::Robot(ros::NodeHandle *nh, Controller robot_ctrl){
     resetTrajServer_ = nh->advertiseService("/reset_traj",
             &Robot::resetTrajCallback, this);
 
+    baseOdomPub_ = nh->advertise<nav_msgs::Odometry>("/odom", 50);
     // PreviewTraj traj(0.68, 320);
     // traj.computeWeight();
     // traj.computeTraj();
@@ -135,6 +136,7 @@ void Robot::spinOnline(int iter, double config[], double jnt_vel[], Vector3d tor
     // quatEKF_->setDt(dt_);
     lieEKF_->setDt(dt_);
     lieEKF_->runFilter(gyro, accelerometer, links_[12]->getPose(), links_[6]->getPose(), links_[12]->getRot(), links_[6]->getRot(), contact, true);
+    baseOdomPublisher(lieEKF_->getGBasePose(), lieEKF_->getGBaseVel(),lieEKF_->getGBaseQuat());
     // cout << gyro(0) << ',' << gyro(1) << ',' << gyro(2) << ",";
     // cout << accelerometer(0) << ',' << accelerometer(1) << ',' << accelerometer(2) << ",";
     // cout << links_[6]->getPose()(0) << ',' << links_[6]->getPose()(1) << ',' << links_[6]->getPose()(2) << ",";
@@ -743,6 +745,28 @@ bool Robot::resetTrajCallback(std_srvs::Empty::Request  &req,
     return true;
 }
 
+void Robot::baseOdomPublisher(Vector3d base_pos, Vector3d base_vel, Quaterniond base_quat){
+    nav_msgs::Odometry odom;
+    odom.header.stamp = ros::Time::now();
+    odom.header.frame_id = "odom";
+
+    odom.pose.pose.position.x = base_pos(0);
+    odom.pose.pose.position.y = base_pos(1);
+    odom.pose.pose.position.z = base_pos(2);
+
+    odom.pose.pose.orientation.x = base_quat.x();
+    odom.pose.pose.orientation.y = base_quat.y();
+    odom.pose.pose.orientation.z = base_quat.z();
+    odom.pose.pose.orientation.w = base_quat.w();
+
+    odom.twist.twist.linear.x = base_vel(0);
+    odom.twist.twist.linear.y = base_vel(1);
+    odom.twist.twist.angular.z = base_vel(2);
+
+    //publish the message
+    baseOdomPub_.publish(odom);
+}
+
 Robot::~Robot(){
     //delete[] links_;
     //delete[] FKCoM_;