include
rm_gazebo
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
40
#include <
gazebo_ros_control/default_robot_hw_sim.h
>
41
#include <
rm_common/hardware_interface/rm_imu_sensor_interface.h
>
42
#include <
rm_common/hardware_interface/robot_state_interface.h
>
43
#include <std_srvs/Trigger.h>
44
45
namespace
rm_gazebo
46
{
47
struct
ImuData
48
{
49
gazebo::physics::LinkPtr
link_ptr
;
50
ros::Time
time_stamp
;
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
59
class
RmRobotHWSim
:
public
gazebo_ros_control::DefaultRobotHWSim
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
{
72
disable_imu_
= !
disable_imu_
;
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
}
78
rm_control::RobotStateInterface
robot_state_interface_
;
79
hardware_interface::ImuSensorInterface
imu_sensor_interface_
;
80
rm_control::RmImuSensorInterface
rm_imu_sensor_interface_
;
81
gazebo::physics::WorldPtr
world_
;
82
std::list<ImuData>
imu_datas_
;
83
ros::ServiceServer
switch_imu_service_
;
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