From cc59e502e08400428f52687fd38bf6ebeea10a06 Mon Sep 17 00:00:00 2001 From: amirhosein-vedadi Date: Fri, 26 Aug 2022 17:43:35 +0430 Subject: [PATCH] odom pub fixed --- model/SurenaV/surena5.wrl | 2 +- project/SurenaV-wrl-house.cnoid | 37 ++++++++++++------- trajectory_planner/CMakeLists.txt | 4 ++ .../include/trajectory_planner/Robot.h | 6 +++ trajectory_planner/package.xml | 4 ++ trajectory_planner/src/Robot.cpp | 24 ++++++++++++ 6 files changed, 63 insertions(+), 14 deletions(-) diff --git a/model/SurenaV/surena5.wrl b/model/SurenaV/surena5.wrl index 40b4a75..b7faafe 100644 --- 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 index 19feb36..f00e748 100644 --- 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,6 +128,9 @@ items: "RLeg_AnkleR_J6": base_frame: 0 offset_frame: 0 + "LLeg_AnkleR_J6": + base_frame: 0 + offset_frame: 0 zmp: [ 0, 0, 0 ] children: - @@ -134,7 +138,6 @@ items: 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,6 +294,11 @@ views: state: element_type: all listingMode: list + current_body_item: 5 + body_items: + - + id: 2 + selected_links: [ 0 ] - id: 8 name: "Link Position" @@ -296,7 +306,7 @@ views: 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 index dc27d52..28a6a40 100644 --- 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 index 182554a..e143a04 100644 --- 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 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 index 0d58a9c..c36de28 100644 --- a/trajectory_planner/package.xml +++ b/trajectory_planner/package.xml @@ -52,12 +52,16 @@ roscpp rospy std_msgs + nav_msgs + geometry_msgs roscpp rospy std_msgs roscpp rospy std_msgs + nav_msgs + geometry_msgs diff --git a/trajectory_planner/src/Robot.cpp b/trajectory_planner/src/Robot.cpp index f749117..c66f96a 100644 --- 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("/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_;