44 ROS_FATAL_STREAM_NAMED(
"cob_gazebo_ros_control",
"A ROS node for Gazebo has not been initialized, unable to load plugin. " 45 <<
"Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
50 if(
sdf_->HasElement(
"robotNamespace"))
60 if (
sdf_->HasElement(
"robotParam"))
70 if(
sdf_->HasElement(
"robotSimType"))
83 #if GAZEBO_MAJOR_VERSION >= 8 84 gazebo::physics::PhysicsEnginePtr physics = gazebo::physics::get_world()->Physics();
86 gazebo::physics::PhysicsEnginePtr physics = gazebo::physics::get_world()->GetPhysicsEngine();
91 if(
sdf_->HasElement(
"controlPeriod"))
99 <<
" s) is faster than the gazebo simulation period ("<<gazebo_period<<
" s).");
104 <<
" s) is slower than the gazebo simulation period ("<<gazebo_period<<
" s).");
110 ROS_DEBUG_STREAM_NAMED(
"cob_gazebo_ros_control",
"Control period not found in URDF/SDF, defaulting to Gazebo period of " 120 if (
sdf_->HasElement(
"eStopTopic"))
122 const std::string e_stop_topic =
sdf_->GetElement(
"eStopTopic")->Get<std::string>();
128 if (
sdf_->HasElement(
"stateValidTopic"))
130 const std::string state_valid_topic =
sdf_->GetElement(
"stateValidTopic")->Get<std::string>();
135 if(
sdf_->HasElement(
"filterJointsParam"))
145 ROS_INFO_STREAM_NAMED(
"cob_gazebo_ros_control",
"Joint-filtering is disabled. The plugin will set up JointHandles for all joints!");
156 ROS_ERROR_NAMED(
"cob_gazebo_ros_control",
"Error parsing URDF in cob_gazebo_ros_control plugin, plugin not active.\n");
187 ROS_FATAL_NAMED(
"cob_gazebo_ros_control",
"Could not initialize robot simulation interface");
198 gazebo::event::Events::ConnectWorldUpdateBegin
204 ROS_FATAL_STREAM_NAMED(
"cob_gazebo_ros_control",
"Failed to create robot simulation interface loader: "<<ex.what());
207 ROS_INFO_NAMED(
"cob_gazebo_ros_control",
"Loaded cob_gazebo_ros_control.");
214 #if GAZEBO_MAJOR_VERSION >= 8 215 gazebo::common::Time gz_time_now = gazebo::physics::get_world()->SimTime();
217 gazebo::common::Time gz_time_now =
parent_model_->GetWorld()->GetSimTime();
219 ros::Time sim_time_ros(gz_time_now.sec, gz_time_now.nsec);
229 last_update_sim_time_ros_ = sim_time_ros;
238 reset_ctrlrs =
false;
250 reset_ctrlrs =
false;
virtual void Load(gazebo::physics::ModelPtr parent, sdf::ElementPtr sdf)
#define ROS_INFO_NAMED(name,...)
ros::NodeHandle model_nh_
#define ROS_DEBUG_STREAM_NAMED(name, args)
#define ROS_ERROR_STREAM_NAMED(name, args)
boost::shared_ptr< controller_manager::ControllerManager > controller_manager_
URDF_EXPORT bool initString(const std::string &xmlstring)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ROSCPP_DECL bool isInitialized()
gazebo::event::ConnectionPtr update_connection_
ros::Subscriber e_stop_sub_
gazebo::physics::ModelPtr parent_model_
#define ROS_INFO_STREAM_NAMED(name, args)
bool enable_joint_filtering_
ros::Time last_update_sim_time_ros_
bool parseTransmissionsFromURDF(const std::string &urdf_string)
ros::Time last_write_sim_time_ros_
std::string filterJointsParam_
ros::Subscriber state_valid_sub_
#define ROS_FATAL_STREAM_NAMED(name, args)
std::vector< transmission_interface::TransmissionInfo > transmissions_
ros::Duration control_period_
std::string robot_namespace_
std::string robot_description_
GZ_REGISTER_MODEL_PLUGIN(HWISwitchGazeboRosControlPlugin)
boost::shared_ptr< cob_gazebo_ros_control::HWISwitchRobotHWSim > hwi_switch_robot_hw_sim_
std::string robot_hw_sim_type_str_
#define ROS_FATAL_NAMED(name,...)
#define ROS_ERROR_NAMED(name,...)
void stateValidCB(const std_msgs::BoolConstPtr &state_valid)
std::string getURDF(std::string param_name) const
#define ROS_WARN_STREAM_NAMED(name, args)
void eStopCB(const std_msgs::BoolConstPtr &e_stop_active)