rm_robot_hw_sim.h
Go to the documentation of this file.
1 /*******************************************************************************
2  * BSD 3-Clause License
3  *
4  * Copyright (c) 2021, Qiayuan Liao
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions are met:
9  *
10  * * Redistributions of source code must retain the above copyright notice, this
11  * list of conditions and the following disclaimer.
12  *
13  * * Redistributions in binary form must reproduce the above copyright notice,
14  * this list of conditions and the following disclaimer in the documentation
15  * and/or other materials provided with the distribution.
16  *
17  * * Neither the name of the copyright holder nor the names of its
18  * contributors may be used to endorse or promote products derived from
19  * this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
22  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
23  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
24  * ARE
25  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
26  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
27  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
28  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
30  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
31  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
32  *******************************************************************************/
33 
34 //
35 // Created by qiayuan on 2/10/21.
36 //
37 
38 #pragma once
39 
43 #include <std_srvs/Trigger.h>
44 
45 namespace rm_gazebo
46 {
47 struct ImuData
48 {
49  gazebo::physics::LinkPtr link_ptr;
51  double ori[4];
52  double ori_cov[9];
53  double angular_vel[3];
54  double angular_vel_cov[9];
55  double linear_acc[3];
56  double linear_acc_cov[9];
57 };
58 
60 {
61 public:
62  bool initSim(const std::string& robot_namespace, ros::NodeHandle model_nh, gazebo::physics::ModelPtr parent_model,
63  const urdf::Model* urdf_model,
64  std::vector<transmission_interface::TransmissionInfo> transmissions) override;
65  void readSim(ros::Time time, ros::Duration period) override;
66 
67 private:
68  void parseImu(XmlRpc::XmlRpcValue& imu_datas, const gazebo::physics::ModelPtr& parent_model);
69 
70  static bool switchImuStatus(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res)
71  {
73  res.success = true;
74  std::string imu_status_message = disable_imu_ ? "disable" : "enable";
75  res.message = "Imu status: " + imu_status_message;
76  return true;
77  }
81  gazebo::physics::WorldPtr world_;
82  std::list<ImuData> imu_datas_;
84  static bool disable_imu_;
85 };
86 
87 } // namespace rm_gazebo
rm_gazebo::ImuData::linear_acc_cov
double linear_acc_cov[9]
Definition: rm_robot_hw_sim.h:118
rm_gazebo::ImuData
Definition: rm_robot_hw_sim.h:78
rm_gazebo::RmRobotHWSim::imu_sensor_interface_
hardware_interface::ImuSensorInterface imu_sensor_interface_
Definition: rm_robot_hw_sim.h:110
default_robot_hw_sim.h
rm_control::RobotStateInterface
rm_gazebo::ImuData::linear_acc
double linear_acc[3]
Definition: rm_robot_hw_sim.h:117
rm_imu_sensor_interface.h
urdf::Model
rm_gazebo::RmRobotHWSim::robot_state_interface_
rm_control::RobotStateInterface robot_state_interface_
Definition: rm_robot_hw_sim.h:109
ros::ServiceServer
rm_gazebo::RmRobotHWSim::disable_imu_
static bool disable_imu_
Definition: rm_robot_hw_sim.h:115
rm_gazebo::RmRobotHWSim::switch_imu_service_
ros::ServiceServer switch_imu_service_
Definition: rm_robot_hw_sim.h:114
rm_gazebo::RmRobotHWSim::readSim
void readSim(ros::Time time, ros::Duration period) override
Definition: rm_robot_hw_sim.cpp:94
rm_gazebo::ImuData::link_ptr
gazebo::physics::LinkPtr link_ptr
Definition: rm_robot_hw_sim.h:111
rm_gazebo::RmRobotHWSim::switchImuStatus
static bool switchImuStatus(std_srvs::TriggerRequest &req, std_srvs::TriggerResponse &res)
Definition: rm_robot_hw_sim.h:101
rm_gazebo::RmRobotHWSim::parseImu
void parseImu(XmlRpc::XmlRpcValue &imu_datas, const gazebo::physics::ModelPtr &parent_model)
Definition: rm_robot_hw_sim.cpp:128
rm_control::RmImuSensorInterface
hardware_interface::ImuSensorInterface
rm_gazebo::RmRobotHWSim::initSim
bool initSim(const std::string &robot_namespace, ros::NodeHandle model_nh, gazebo::physics::ModelPtr parent_model, const urdf::Model *urdf_model, std::vector< transmission_interface::TransmissionInfo > transmissions) override
Definition: rm_robot_hw_sim.cpp:75
rm_gazebo::RmRobotHWSim::world_
gazebo::physics::WorldPtr world_
Definition: rm_robot_hw_sim.h:112
rm_gazebo::ImuData::angular_vel
double angular_vel[3]
Definition: rm_robot_hw_sim.h:115
rm_gazebo::RmRobotHWSim::rm_imu_sensor_interface_
rm_control::RmImuSensorInterface rm_imu_sensor_interface_
Definition: rm_robot_hw_sim.h:111
rm_gazebo::ImuData::time_stamp
ros::Time time_stamp
Definition: rm_robot_hw_sim.h:112
robot_state_interface.h
ros::Time
rm_gazebo::ImuData::ori
double ori[4]
Definition: rm_robot_hw_sim.h:113
rm_gazebo
Definition: rm_robot_hw_sim.h:45
rm_gazebo::RmRobotHWSim::imu_datas_
std::list< ImuData > imu_datas_
Definition: rm_robot_hw_sim.h:113
rm_gazebo::RmRobotHWSim
Definition: rm_robot_hw_sim.h:90
rm_gazebo::ImuData::ori_cov
double ori_cov[9]
Definition: rm_robot_hw_sim.h:114
gazebo_ros_control::DefaultRobotHWSim
ros::Duration
rm_gazebo::ImuData::angular_vel_cov
double angular_vel_cov[9]
Definition: rm_robot_hw_sim.h:116
XmlRpc::XmlRpcValue
ros::NodeHandle


rm_gazebo
Author(s): Qiayuan Liao
autogenerated on Tue May 6 2025 02:23:42