23 #include <gazebo/common/Events.hh> 24 #include <gazebo/gazebo_config.h> 31 plugin_loaded_(false),
33 pub_link_states_connection_count_(0),
34 pub_model_states_connection_count_(0),
35 pub_performance_metrics_connection_count_(0),
36 physics_reconfigure_initialized_(false),
37 pub_clock_frequency_(0),
125 ROS_ERROR_NAMED(
"api_plugin",
"Something other than this gazebo_ros_api plugin started ros::init(...), command line arguments may not be parsed properly.");
157 ROS_INFO_NAMED(
"api_plugin",
"Finished loading Gazebo ROS API Plugin.");
174 world_ = gazebo::physics::get_world(world_name);
179 ROS_FATAL_NAMED(
"api_plugin",
"cannot load gazebo ros api server plugin, physics::get_world() fails to return world");
183 gazebonode_ = gazebo::transport::NodePtr(
new gazebo::transport::Node());
209 #ifdef GAZEBO_ROS_HAS_PERFORMANCE_METRICS 212 gazebo_msgs::PerformanceMetrics msg_ros;
214 msg_ros.real_time_factor = msg->real_time_factor();
215 for (
auto sensor: msg->sensor())
218 sensor_msgs.sim_update_rate = sensor.sim_update_rate();
219 sensor_msgs.real_update_rate = sensor.real_update_rate();
220 sensor_msgs.name = sensor.name();
222 if (sensor.has_fps())
224 sensor_msgs.fps = sensor.fps();
227 sensor_msgs.fps = -1;
230 msg_ros.sensors.push_back(sensor_msgs);
239 static const double timeout = 0.001;
249 pub_clock_ =
nh_->advertise<rosgraph_msgs::Clock>(
"/clock",10);
252 std::string spawn_sdf_model_service_name(
"spawn_sdf_model");
254 ros::AdvertiseServiceOptions::create<gazebo_msgs::SpawnModel>(
255 spawn_sdf_model_service_name,
261 std::string spawn_urdf_model_service_name(
"spawn_urdf_model");
263 ros::AdvertiseServiceOptions::create<gazebo_msgs::SpawnModel>(
264 spawn_urdf_model_service_name,
270 std::string delete_model_service_name(
"delete_model");
272 ros::AdvertiseServiceOptions::create<gazebo_msgs::DeleteModel>(
273 delete_model_service_name,
279 std::string delete_light_service_name(
"delete_light");
281 ros::AdvertiseServiceOptions::create<gazebo_msgs::DeleteLight>(
282 delete_light_service_name,
288 std::string get_model_properties_service_name(
"get_model_properties");
290 ros::AdvertiseServiceOptions::create<gazebo_msgs::GetModelProperties>(
291 get_model_properties_service_name,
297 std::string get_model_state_service_name(
"get_model_state");
299 ros::AdvertiseServiceOptions::create<gazebo_msgs::GetModelState>(
300 get_model_state_service_name,
306 std::string get_world_properties_service_name(
"get_world_properties");
308 ros::AdvertiseServiceOptions::create<gazebo_msgs::GetWorldProperties>(
309 get_world_properties_service_name,
315 std::string get_joint_properties_service_name(
"get_joint_properties");
317 ros::AdvertiseServiceOptions::create<gazebo_msgs::GetJointProperties>(
318 get_joint_properties_service_name,
324 std::string get_link_properties_service_name(
"get_link_properties");
326 ros::AdvertiseServiceOptions::create<gazebo_msgs::GetLinkProperties>(
327 get_link_properties_service_name,
333 std::string get_link_state_service_name(
"get_link_state");
335 ros::AdvertiseServiceOptions::create<gazebo_msgs::GetLinkState>(
336 get_link_state_service_name,
342 std::string get_light_properties_service_name(
"get_light_properties");
344 ros::AdvertiseServiceOptions::create<gazebo_msgs::GetLightProperties>(
345 get_light_properties_service_name,
351 std::string set_light_properties_service_name(
"set_light_properties");
353 ros::AdvertiseServiceOptions::create<gazebo_msgs::SetLightProperties>(
354 set_light_properties_service_name,
360 std::string get_physics_properties_service_name(
"get_physics_properties");
362 ros::AdvertiseServiceOptions::create<gazebo_msgs::GetPhysicsProperties>(
363 get_physics_properties_service_name,
370 ros::AdvertiseOptions::create<gazebo_msgs::LinkStates>(
379 ros::AdvertiseOptions::create<gazebo_msgs::ModelStates>(
386 #ifdef GAZEBO_ROS_HAS_PERFORMANCE_METRICS 389 ros::AdvertiseOptions::create<gazebo_msgs::PerformanceMetrics>(
390 "performance_metrics",10,
391 boost::bind(&GazeboRosApiPlugin::onPerformanceMetricsConnect,
this),
392 boost::bind(&GazeboRosApiPlugin::onPerformanceMetricsDisconnect,
this),
398 std::string set_link_properties_service_name(
"set_link_properties");
400 ros::AdvertiseServiceOptions::create<gazebo_msgs::SetLinkProperties>(
401 set_link_properties_service_name,
407 std::string set_physics_properties_service_name(
"set_physics_properties");
409 ros::AdvertiseServiceOptions::create<gazebo_msgs::SetPhysicsProperties>(
410 set_physics_properties_service_name,
416 std::string set_model_state_service_name(
"set_model_state");
418 ros::AdvertiseServiceOptions::create<gazebo_msgs::SetModelState>(
419 set_model_state_service_name,
425 std::string set_model_configuration_service_name(
"set_model_configuration");
427 ros::AdvertiseServiceOptions::create<gazebo_msgs::SetModelConfiguration>(
428 set_model_configuration_service_name,
434 std::string set_joint_properties_service_name(
"set_joint_properties");
436 ros::AdvertiseServiceOptions::create<gazebo_msgs::SetJointProperties>(
437 set_joint_properties_service_name,
443 std::string set_link_state_service_name(
"set_link_state");
445 ros::AdvertiseServiceOptions::create<gazebo_msgs::SetLinkState>(
446 set_link_state_service_name,
454 ros::SubscribeOptions::create<gazebo_msgs::LinkState>(
462 ros::SubscribeOptions::create<gazebo_msgs::ModelState>(
463 "set_model_state",10,
469 std::string pause_physics_service_name(
"pause_physics");
471 ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
472 pause_physics_service_name,
478 std::string unpause_physics_service_name(
"unpause_physics");
480 ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
481 unpause_physics_service_name,
487 std::string apply_body_wrench_service_name(
"apply_body_wrench");
489 ros::AdvertiseServiceOptions::create<gazebo_msgs::ApplyBodyWrench>(
490 apply_body_wrench_service_name,
496 std::string apply_joint_effort_service_name(
"apply_joint_effort");
498 ros::AdvertiseServiceOptions::create<gazebo_msgs::ApplyJointEffort>(
499 apply_joint_effort_service_name,
505 std::string clear_joint_forces_service_name(
"clear_joint_forces");
507 ros::AdvertiseServiceOptions::create<gazebo_msgs::JointRequest>(
508 clear_joint_forces_service_name,
514 std::string clear_body_wrenches_service_name(
"clear_body_wrenches");
516 ros::AdvertiseServiceOptions::create<gazebo_msgs::BodyRequest>(
517 clear_body_wrenches_service_name,
523 std::string reset_simulation_service_name(
"reset_simulation");
525 ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
526 reset_simulation_service_name,
532 std::string reset_world_service_name(
"reset_world");
534 ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
535 reset_world_service_name,
542 if(!(
nh_->hasParam(
"/use_sim_time")))
543 nh_->setParam(
"/use_sim_time",
true);
547 #if GAZEBO_MAJOR_VERSION >= 8 568 #ifdef GAZEBO_ROS_HAS_PERFORMANCE_METRICS 569 void GazeboRosApiPlugin::onPerformanceMetricsConnect()
575 &GazeboRosApiPlugin::onPerformanceMetrics,
this);
579 void GazeboRosApiPlugin::onPerformanceMetricsDisconnect()
586 ROS_ERROR_NAMED(
"api_plugin",
"One too many disconnect from pub_performance_metrics_ in gazebo_ros.cpp? something weird");
598 ROS_ERROR_NAMED(
"api_plugin",
"One too many disconnect from pub_link_states_ in gazebo_ros.cpp? something weird");
609 ROS_ERROR_NAMED(
"api_plugin",
"One too many disconnect from pub_model_states_ in gazebo_ros.cpp? something weird");
614 gazebo_msgs::SpawnModel::Response &res)
620 std::string model_xml = req.model_xml;
624 ROS_ERROR_NAMED(
"api_plugin",
"SpawnModel: Failure - entity format is invalid.");
626 res.status_message =
"SpawnModel: Failure - entity format is invalid.";
634 std::string open_bracket(
"<?");
635 std::string close_bracket(
"?>");
636 size_t pos1 = model_xml.find(open_bracket,0);
637 size_t pos2 = model_xml.find(close_bracket,0);
638 if (pos1 != std::string::npos && pos2 != std::string::npos)
639 model_xml.replace(pos1,pos2-pos1+2,std::string(
""));
644 std::string open_comment(
"<!--");
645 std::string close_comment(
"-->");
647 while((pos1 = model_xml.find(open_comment,0)) != std::string::npos){
648 size_t pos2 = model_xml.find(close_comment,0);
649 if (pos2 != std::string::npos)
650 model_xml.replace(pos1,pos2-pos1+3,std::string(
""));
656 std::string package_prefix(
"package://");
657 size_t pos1 = model_xml.find(package_prefix,0);
658 while (pos1 != std::string::npos)
660 size_t pos2 = model_xml.find(
"/", pos1+10);
662 if (pos2 == std::string::npos || pos1 >= pos2)
668 std::string package_name = model_xml.substr(pos1+10,pos2-pos1-10);
671 if (package_path.empty())
673 ROS_FATAL_NAMED(
"api_plugin",
"Package[%s] does not have a path",package_name.c_str());
675 res.status_message =
"urdf reference package name does not exist: " + package_name;
678 ROS_DEBUG_ONCE_NAMED(
"api_plugin",
"Package name [%s] has path [%s]", package_name.c_str(), package_path.c_str());
680 model_xml.replace(pos1,(pos2-pos1),package_path);
681 pos1 = model_xml.find(package_prefix, pos1);
686 req.model_xml = model_xml;
693 gazebo_msgs::SpawnModel::Response &res)
696 std::string model_name = req.model_name;
702 ignition::math::Vector3d initial_xyz(req.initial_pose.position.x,req.initial_pose.position.y,req.initial_pose.position.z);
704 ignition::math::Quaterniond initial_q(req.initial_pose.orientation.w,req.initial_pose.orientation.x,req.initial_pose.orientation.y,req.initial_pose.orientation.z);
707 #if GAZEBO_MAJOR_VERSION >= 8 708 gazebo::physics::EntityPtr frame =
world_->EntityByName(req.reference_frame);
710 gazebo::physics::EntityPtr frame =
world_->GetEntity(req.reference_frame);
715 #if GAZEBO_MAJOR_VERSION >= 8 716 ignition::math::Pose3d frame_pose = frame->WorldPose();
718 ignition::math::Pose3d frame_pose = frame->GetWorldPose().Ign();
720 initial_xyz = frame_pose.Pos() + frame_pose.Rot().RotateVector(initial_xyz);
721 initial_q = frame_pose.Rot() * initial_q;
725 else if (req.reference_frame ==
"" || req.reference_frame ==
"world" || req.reference_frame ==
"map" || req.reference_frame ==
"/map")
727 ROS_DEBUG_NAMED(
"api_plugin",
"SpawnModel: reference_frame is empty/world/map, using inertial frame");
732 res.status_message =
"SpawnModel: reference reference_frame not found, did you forget to scope the link by model name?";
737 std::string model_xml = req.model_xml;
745 TiXmlDocument gazebo_model_xml;
746 gazebo_model_xml.Parse(model_xml.c_str());
749 if (
isSDF(model_xml))
759 TiXmlNode* model_tixml = gazebo_model_xml.FirstChild(
"sdf");
760 model_tixml = (!model_tixml) ?
761 gazebo_model_xml.FirstChild(
"gazebo") : model_tixml;
768 ROS_WARN_NAMED(
"api_plugin",
"Unable to add robot namespace to xml");
772 else if (
isURDF(model_xml))
782 TiXmlNode* model_tixml = gazebo_model_xml.FirstChild(
"robot");
789 ROS_WARN_NAMED(
"api_plugin",
"Unable to add robot namespace to xml");
795 ROS_ERROR_NAMED(
"api_plugin",
"GazeboRosApiPlugin SpawnModel Failure: input xml format not recognized");
797 res.status_message =
"GazeboRosApiPlugin SpawnModel Failure: input model_xml not SDF or URDF, or cannot be converted to Gazebo compatible format.";
806 gazebo_msgs::DeleteModel::Response &res)
809 #if GAZEBO_MAJOR_VERSION >= 8 810 gazebo::physics::ModelPtr model =
world_->ModelByName(req.model_name);
812 gazebo::physics::ModelPtr model =
world_->GetModel(req.model_name);
816 ROS_ERROR_NAMED(
"api_plugin",
"DeleteModel: model [%s] does not exist",req.model_name.c_str());
818 res.status_message =
"DeleteModel: model does not exist";
823 for (
unsigned int i = 0 ; i < model->GetChildCount(); i ++)
825 gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast<gazebo::physics::Link>(model->GetChild(i));
834 gazebo::physics::Joint_V joints = model->GetJoints();
835 for (
unsigned int i=0;i< joints.size(); i++)
842 gazebo::msgs::Request *msg = gazebo::msgs::CreateRequest(
"entity_delete",req.model_name);
855 res.status_message =
"DeleteModel: Model pushed to delete queue, but delete service timed out waiting for model to disappear from simulation";
860 #if GAZEBO_MAJOR_VERSION >= 8 861 if (!
world_->ModelByName(req.model_name))
break;
863 if (!
world_->GetModel(req.model_name))
break;
866 ROS_DEBUG_NAMED(
"api_plugin",
"Waiting for model deletion (%s)",req.model_name.c_str());
872 res.status_message =
"DeleteModel: successfully deleted model";
877 gazebo_msgs::DeleteLight::Response &res)
879 #if GAZEBO_MAJOR_VERSION >= 8 880 gazebo::physics::LightPtr phy_light =
world_->LightByName(req.light_name);
882 gazebo::physics::LightPtr phy_light =
world_->Light(req.light_name);
885 if (phy_light == NULL)
888 res.status_message =
"DeleteLight: Requested light " + req.light_name +
" not found!";
892 gazebo::msgs::Request* msg = gazebo::msgs::CreateRequest(
"entity_delete", req.light_name);
899 for (
int i = 0; i < 100; i++)
901 #if GAZEBO_MAJOR_VERSION >= 8 902 phy_light =
world_->LightByName(req.light_name);
904 phy_light =
world_->Light(req.light_name);
906 if (phy_light == NULL)
909 res.status_message =
"DeleteLight: " + req.light_name +
" successfully deleted";
917 res.status_message =
"DeleteLight: Timeout reached while removing light \"" + req.light_name
924 gazebo_msgs::GetModelState::Response &res)
926 #if GAZEBO_MAJOR_VERSION >= 8 927 gazebo::physics::ModelPtr model =
world_->ModelByName(req.model_name);
928 gazebo::physics::EntityPtr frame =
world_->EntityByName(req.relative_entity_name);
930 gazebo::physics::ModelPtr model =
world_->GetModel(req.model_name);
931 gazebo::physics::EntityPtr frame =
world_->GetEntity(req.relative_entity_name);
935 ROS_ERROR_NAMED(
"api_plugin",
"GetModelState: model [%s] does not exist",req.model_name.c_str());
937 res.status_message =
"GetModelState: model does not exist";
957 res.header.seq = it->second;
960 res.header.frame_id = req.relative_entity_name;
964 #if GAZEBO_MAJOR_VERSION >= 8 965 ignition::math::Pose3d model_pose = model->WorldPose();
966 ignition::math::Vector3d model_linear_vel = model->WorldLinearVel();
967 ignition::math::Vector3d model_angular_vel = model->WorldAngularVel();
969 ignition::math::Pose3d model_pose = model->GetWorldPose().Ign();
970 ignition::math::Vector3d model_linear_vel = model->GetWorldLinearVel().Ign();
971 ignition::math::Vector3d model_angular_vel = model->GetWorldAngularVel().Ign();
973 ignition::math::Vector3d model_pos = model_pose.Pos();
974 ignition::math::Quaterniond model_rot = model_pose.Rot();
981 #if GAZEBO_MAJOR_VERSION >= 8 982 ignition::math::Pose3d frame_pose = frame->WorldPose();
983 ignition::math::Vector3d frame_vpos = frame->WorldLinearVel();
984 ignition::math::Vector3d frame_veul = frame->WorldAngularVel();
986 ignition::math::Pose3d frame_pose = frame->GetWorldPose().Ign();
987 ignition::math::Vector3d frame_vpos = frame->GetWorldLinearVel().Ign();
988 ignition::math::Vector3d frame_veul = frame->GetWorldAngularVel().Ign();
990 ignition::math::Pose3d model_rel_pose = model_pose - frame_pose;
991 model_pos = model_rel_pose.Pos();
992 model_rot = model_rel_pose.Rot();
994 model_linear_vel = frame_pose.Rot().RotateVectorReverse(model_linear_vel - frame_vpos);
995 model_angular_vel = frame_pose.Rot().RotateVectorReverse(model_angular_vel - frame_veul);
998 else if (req.relative_entity_name ==
"" || req.relative_entity_name ==
"world" || req.relative_entity_name ==
"map" || req.relative_entity_name ==
"/map")
1000 ROS_DEBUG_NAMED(
"api_plugin",
"GetModelState: relative_entity_name is empty/world/map, using inertial frame");
1004 res.success =
false;
1005 res.status_message =
"GetModelState: reference relative_entity_name not found, did you forget to scope the body by model name?";
1010 res.pose.position.x = model_pos.X();
1011 res.pose.position.y = model_pos.Y();
1012 res.pose.position.z = model_pos.Z();
1013 res.pose.orientation.w = model_rot.W();
1014 res.pose.orientation.x = model_rot.X();
1015 res.pose.orientation.y = model_rot.Y();
1016 res.pose.orientation.z = model_rot.Z();
1018 res.twist.linear.x = model_linear_vel.X();
1019 res.twist.linear.y = model_linear_vel.Y();
1020 res.twist.linear.z = model_linear_vel.Z();
1021 res.twist.angular.x = model_angular_vel.X();
1022 res.twist.angular.y = model_angular_vel.Y();
1023 res.twist.angular.z = model_angular_vel.Z();
1026 res.status_message =
"GetModelState: got properties";
1033 gazebo_msgs::GetModelProperties::Response &res)
1035 #if GAZEBO_MAJOR_VERSION >= 8 1036 gazebo::physics::ModelPtr model =
world_->ModelByName(req.model_name);
1038 gazebo::physics::ModelPtr model =
world_->GetModel(req.model_name);
1042 ROS_ERROR_NAMED(
"api_plugin",
"GetModelProperties: model [%s] does not exist",req.model_name.c_str());
1043 res.success =
false;
1044 res.status_message =
"GetModelProperties: model does not exist";
1050 gazebo::physics::ModelPtr parent_model = boost::dynamic_pointer_cast<gazebo::physics::Model>(model->GetParent());
1051 if (parent_model) res.parent_model_name = parent_model->GetName();
1054 res.body_names.clear();
1055 res.geom_names.clear();
1056 for (
unsigned int i = 0 ; i < model->GetChildCount(); i ++)
1058 gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast<gazebo::physics::Link>(model->GetChild(i));
1061 res.body_names.push_back(body->GetName());
1063 for (
unsigned int j = 0; j < body->GetChildCount() ; j++)
1065 gazebo::physics::CollisionPtr geom = boost::dynamic_pointer_cast<gazebo::physics::Collision>(body->GetChild(j));
1067 res.geom_names.push_back(geom->GetName());
1073 res.joint_names.clear();
1075 gazebo::physics::Joint_V joints = model->GetJoints();
1076 for (
unsigned int i=0;i< joints.size(); i++)
1077 res.joint_names.push_back( joints[i]->GetName() );
1080 res.child_model_names.clear();
1081 for (
unsigned int j = 0; j < model->GetChildCount(); j++)
1083 gazebo::physics::ModelPtr child_model = boost::dynamic_pointer_cast<gazebo::physics::Model>(model->GetChild(j));
1085 res.child_model_names.push_back(child_model->GetName() );
1089 res.is_static = model->IsStatic();
1092 res.status_message =
"GetModelProperties: got properties";
1099 gazebo_msgs::GetWorldProperties::Response &res)
1101 res.model_names.clear();
1102 #if GAZEBO_MAJOR_VERSION >= 8 1103 res.sim_time =
world_->SimTime().Double();
1104 for (
unsigned int i = 0; i <
world_->ModelCount(); i ++)
1105 res.model_names.push_back(
world_->ModelByIndex(i)->GetName());
1107 res.sim_time =
world_->GetSimTime().Double();
1108 for (
unsigned int i = 0; i <
world_->GetModelCount(); i ++)
1109 res.model_names.push_back(
world_->GetModel(i)->GetName());
1111 gzerr <<
"disablign rendering has not been implemented, rendering is always enabled\n";
1112 res.rendering_enabled =
true;
1114 res.status_message =
"GetWorldProperties: got properties";
1119 gazebo_msgs::GetJointProperties::Response &res)
1121 gazebo::physics::JointPtr joint;
1122 #if GAZEBO_MAJOR_VERSION >= 8 1123 for (
unsigned int i = 0; i <
world_->ModelCount(); i ++)
1125 joint =
world_->ModelByIndex(i)->GetJoint(req.joint_name);
1127 for (
unsigned int i = 0; i <
world_->GetModelCount(); i ++)
1129 joint =
world_->GetModel(i)->GetJoint(req.joint_name);
1136 res.success =
false;
1137 res.status_message =
"GetJointProperties: joint not found";
1143 res.type = res.REVOLUTE;
1145 res.damping.clear();
1148 res.position.clear();
1149 #if GAZEBO_MAJOR_VERSION >= 8 1150 res.position.push_back(joint->Position(0));
1152 res.position.push_back(joint->GetAngle(0).Radian());
1156 res.rate.push_back(joint->GetVelocity(0));
1159 res.status_message =
"GetJointProperties: got properties";
1165 gazebo_msgs::GetLinkProperties::Response &res)
1167 #if GAZEBO_MAJOR_VERSION >= 8 1168 gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast<gazebo::physics::Link>(
world_->EntityByName(req.link_name));
1170 gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast<gazebo::physics::Link>(
world_->GetEntity(req.link_name));
1174 res.success =
false;
1175 res.status_message =
"GetLinkProperties: link not found, did you forget to scope the link by model name?";
1181 res.gravity_mode = body->GetGravityMode();
1183 gazebo::physics::InertialPtr inertia = body->GetInertial();
1185 #if GAZEBO_MAJOR_VERSION >= 8 1186 res.mass = body->GetInertial()->Mass();
1188 res.ixx = inertia->IXX();
1189 res.iyy = inertia->IYY();
1190 res.izz = inertia->IZZ();
1191 res.ixy = inertia->IXY();
1192 res.ixz = inertia->IXZ();
1193 res.iyz = inertia->IYZ();
1195 ignition::math::Vector3d com = body->GetInertial()->CoG();
1197 res.mass = body->GetInertial()->GetMass();
1199 res.ixx = inertia->GetIXX();
1200 res.iyy = inertia->GetIYY();
1201 res.izz = inertia->GetIZZ();
1202 res.ixy = inertia->GetIXY();
1203 res.ixz = inertia->GetIXZ();
1204 res.iyz = inertia->GetIYZ();
1206 ignition::math::Vector3d com = body->GetInertial()->GetCoG().Ign();
1208 res.com.position.x = com.X();
1209 res.com.position.y = com.Y();
1210 res.com.position.z = com.Z();
1211 res.com.orientation.x = 0;
1212 res.com.orientation.y = 0;
1213 res.com.orientation.z = 0;
1214 res.com.orientation.w = 1;
1217 res.status_message =
"GetLinkProperties: got properties";
1223 gazebo_msgs::GetLinkState::Response &res)
1225 #if GAZEBO_MAJOR_VERSION >= 8 1226 gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast<gazebo::physics::Link>(
world_->EntityByName(req.link_name));
1227 gazebo::physics::EntityPtr frame =
world_->EntityByName(req.reference_frame);
1229 gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast<gazebo::physics::Link>(
world_->GetEntity(req.link_name));
1230 gazebo::physics::EntityPtr frame =
world_->GetEntity(req.reference_frame);
1235 res.success =
false;
1236 res.status_message =
"GetLinkState: link not found, did you forget to scope the link by model name?";
1242 #if GAZEBO_MAJOR_VERSION >= 8 1243 ignition::math::Pose3d body_pose = body->WorldPose();
1244 ignition::math::Vector3d body_vpos = body->WorldLinearVel();
1245 ignition::math::Vector3d body_veul = body->WorldAngularVel();
1247 ignition::math::Pose3d body_pose = body->GetWorldPose().Ign();
1248 ignition::math::Vector3d body_vpos = body->GetWorldLinearVel().Ign();
1249 ignition::math::Vector3d body_veul = body->GetWorldAngularVel().Ign();
1255 #if GAZEBO_MAJOR_VERSION >= 8 1256 ignition::math::Pose3d frame_pose = frame->WorldPose();
1257 ignition::math::Vector3d frame_vpos = frame->WorldLinearVel();
1258 ignition::math::Vector3d frame_veul = frame->WorldAngularVel();
1260 ignition::math::Pose3d frame_pose = frame->GetWorldPose().Ign();
1261 ignition::math::Vector3d frame_vpos = frame->GetWorldLinearVel().Ign();
1262 ignition::math::Vector3d frame_veul = frame->GetWorldAngularVel().Ign();
1264 body_pose = body_pose - frame_pose;
1266 body_vpos = frame_pose.Rot().RotateVectorReverse(body_vpos - frame_vpos);
1267 body_veul = frame_pose.Rot().RotateVectorReverse(body_veul - frame_veul);
1270 else if (req.reference_frame ==
"" || req.reference_frame ==
"world" || req.reference_frame ==
"map" || req.reference_frame ==
"/map")
1272 ROS_DEBUG_NAMED(
"api_plugin",
"GetLinkState: reference_frame is empty/world/map, using inertial frame");
1276 res.success =
false;
1277 res.status_message =
"GetLinkState: reference reference_frame not found, did you forget to scope the link by model name?";
1281 res.link_state.link_name = req.link_name;
1282 res.link_state.pose.position.x = body_pose.Pos().X();
1283 res.link_state.pose.position.y = body_pose.Pos().Y();
1284 res.link_state.pose.position.z = body_pose.Pos().Z();
1285 res.link_state.pose.orientation.x = body_pose.Rot().X();
1286 res.link_state.pose.orientation.y = body_pose.Rot().Y();
1287 res.link_state.pose.orientation.z = body_pose.Rot().Z();
1288 res.link_state.pose.orientation.w = body_pose.Rot().W();
1289 res.link_state.twist.linear.x = body_vpos.X();
1290 res.link_state.twist.linear.y = body_vpos.Y();
1291 res.link_state.twist.linear.z = body_vpos.Z();
1292 res.link_state.twist.angular.x = body_veul.X();
1293 res.link_state.twist.angular.y = body_veul.Y();
1294 res.link_state.twist.angular.z = body_veul.Z();
1295 res.link_state.reference_frame = req.reference_frame;
1298 res.status_message =
"GetLinkState: got state";
1303 gazebo_msgs::GetLightProperties::Response &res)
1305 #if GAZEBO_MAJOR_VERSION >= 8 1306 gazebo::physics::LightPtr phy_light =
world_->LightByName(req.light_name);
1308 gazebo::physics::LightPtr phy_light =
world_->Light(req.light_name);
1311 if (phy_light == NULL)
1313 res.success =
false;
1314 res.status_message =
"getLightProperties: Requested light " + req.light_name +
" not found!";
1318 gazebo::msgs::Light light;
1319 phy_light->FillMsg(light);
1321 res.diffuse.r = light.diffuse().r();
1322 res.diffuse.g = light.diffuse().g();
1323 res.diffuse.b = light.diffuse().b();
1324 res.diffuse.a = light.diffuse().a();
1326 res.attenuation_constant = light.attenuation_constant();
1327 res.attenuation_linear = light.attenuation_linear();
1328 res.attenuation_quadratic = light.attenuation_quadratic();
1337 gazebo_msgs::SetLightProperties::Response &res)
1339 #if GAZEBO_MAJOR_VERSION >= 8 1340 gazebo::physics::LightPtr phy_light =
world_->LightByName(req.light_name);
1342 gazebo::physics::LightPtr phy_light =
world_->Light(req.light_name);
1345 if (phy_light == NULL)
1347 res.success =
false;
1348 res.status_message =
"setLightProperties: Requested light " + req.light_name +
" not found!";
1352 gazebo::msgs::Light light;
1354 phy_light->FillMsg(light);
1356 light.mutable_diffuse()->set_r(req.diffuse.r);
1357 light.mutable_diffuse()->set_g(req.diffuse.g);
1358 light.mutable_diffuse()->set_b(req.diffuse.b);
1359 light.mutable_diffuse()->set_a(req.diffuse.a);
1361 light.set_attenuation_constant(req.attenuation_constant);
1362 light.set_attenuation_linear(req.attenuation_linear);
1363 light.set_attenuation_quadratic(req.attenuation_quadratic);
1374 gazebo_msgs::SetLinkProperties::Response &res)
1376 #if GAZEBO_MAJOR_VERSION >= 8 1377 gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast<gazebo::physics::Link>(
world_->EntityByName(req.link_name));
1379 gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast<gazebo::physics::Link>(
world_->GetEntity(req.link_name));
1383 res.success =
false;
1384 res.status_message =
"SetLinkProperties: link not found, did you forget to scope the link by model name?";
1389 gazebo::physics::InertialPtr mass = body->GetInertial();
1392 mass->SetCoG(ignition::math::Vector3d(req.com.position.x,req.com.position.y,req.com.position.z));
1393 mass->SetInertiaMatrix(req.ixx,req.iyy,req.izz,req.ixy,req.ixz,req.iyz);
1394 mass->SetMass(req.mass);
1395 body->SetGravityMode(req.gravity_mode);
1398 res.status_message =
"SetLinkProperties: properties set";
1404 gazebo_msgs::SetPhysicsProperties::Response &res)
1407 bool is_paused =
world_->IsPaused();
1409 world_->SetGravity(ignition::math::Vector3d(req.gravity.x,req.gravity.y,req.gravity.z));
1412 #if GAZEBO_MAJOR_VERSION >= 8 1413 gazebo::physics::PhysicsEnginePtr pe = (
world_->Physics());
1415 gazebo::physics::PhysicsEnginePtr pe = (
world_->GetPhysicsEngine());
1417 pe->SetMaxStepSize(req.time_step);
1418 pe->SetRealTimeUpdateRate(req.max_update_rate);
1420 if (pe->GetType() ==
"ode")
1423 pe->SetAutoDisableFlag(req.ode_config.auto_disable_bodies);
1424 pe->SetParam(
"precon_iters",
int(req.ode_config.sor_pgs_precon_iters));
1425 pe->SetParam(
"iters",
int(req.ode_config.sor_pgs_iters));
1426 pe->SetParam(
"sor", req.ode_config.sor_pgs_w);
1427 pe->SetParam(
"cfm", req.ode_config.cfm);
1428 pe->SetParam(
"erp", req.ode_config.erp);
1429 pe->SetParam(
"contact_surface_layer",
1430 req.ode_config.contact_surface_layer);
1431 pe->SetParam(
"contact_max_correcting_vel",
1432 req.ode_config.contact_max_correcting_vel);
1433 pe->SetParam(
"max_contacts",
int(req.ode_config.max_contacts));
1435 world_->SetPaused(is_paused);
1438 res.status_message =
"physics engine updated";
1443 ROS_ERROR_NAMED(
"api_plugin",
"ROS set_physics_properties service call does not yet support physics engine [%s].", pe->GetType().c_str());
1444 res.success =
false;
1445 res.status_message =
"Physics engine [" + pe->GetType() +
"]: set_physics_properties not supported.";
1451 gazebo_msgs::GetPhysicsProperties::Response &res)
1454 #if GAZEBO_MAJOR_VERSION >= 8 1455 gazebo::physics::PhysicsEnginePtr pe = (
world_->Physics());
1457 gazebo::physics::PhysicsEnginePtr pe = (
world_->GetPhysicsEngine());
1459 res.time_step = pe->GetMaxStepSize();
1460 res.pause =
world_->IsPaused();
1461 res.max_update_rate = pe->GetRealTimeUpdateRate();
1462 ignition::math::Vector3d gravity =
world_->Gravity();
1463 res.gravity.x = gravity.X();
1464 res.gravity.y = gravity.Y();
1465 res.gravity.z = gravity.Z();
1468 if (pe->GetType() ==
"ode")
1470 res.ode_config.auto_disable_bodies =
1471 pe->GetAutoDisableFlag();
1472 res.ode_config.sor_pgs_precon_iters = boost::any_cast<
int>(
1473 pe->GetParam(
"precon_iters"));
1474 res.ode_config.sor_pgs_iters = boost::any_cast<
int>(
1475 pe->GetParam(
"iters"));
1476 res.ode_config.sor_pgs_w = boost::any_cast<
double>(
1477 pe->GetParam(
"sor"));
1478 res.ode_config.contact_surface_layer = boost::any_cast<
double>(
1479 pe->GetParam(
"contact_surface_layer"));
1480 res.ode_config.contact_max_correcting_vel = boost::any_cast<
double>(
1481 pe->GetParam(
"contact_max_correcting_vel"));
1482 res.ode_config.cfm = boost::any_cast<
double>(
1483 pe->GetParam(
"cfm"));
1484 res.ode_config.erp = boost::any_cast<
double>(
1485 pe->GetParam(
"erp"));
1486 res.ode_config.max_contacts = boost::any_cast<
int>(
1487 pe->GetParam(
"max_contacts"));
1490 res.status_message =
"GetPhysicsProperties: got properties";
1495 ROS_ERROR_NAMED(
"api_plugin",
"ROS get_physics_properties service call does not yet support physics engine [%s].", pe->GetType().c_str());
1496 res.success =
false;
1497 res.status_message =
"Physics engine [" + pe->GetType() +
"]: get_physics_properties not supported.";
1503 gazebo_msgs::SetJointProperties::Response &res)
1506 gazebo::physics::JointPtr joint;
1507 #if GAZEBO_MAJOR_VERSION >= 8 1508 for (
unsigned int i = 0; i <
world_->ModelCount(); i ++)
1510 joint =
world_->ModelByIndex(i)->GetJoint(req.joint_name);
1512 for (
unsigned int i = 0; i <
world_->GetModelCount(); i ++)
1514 joint =
world_->GetModel(i)->GetJoint(req.joint_name);
1521 res.success =
false;
1522 res.status_message =
"SetJointProperties: joint not found";
1527 for(
unsigned int i=0;i< req.ode_joint_config.damping.size();i++)
1528 joint->SetDamping(i,req.ode_joint_config.damping[i]);
1529 for(
unsigned int i=0;i< req.ode_joint_config.hiStop.size();i++)
1530 joint->SetParam(
"hi_stop",i,req.ode_joint_config.hiStop[i]);
1531 for(
unsigned int i=0;i< req.ode_joint_config.loStop.size();i++)
1532 joint->SetParam(
"lo_stop",i,req.ode_joint_config.loStop[i]);
1533 for(
unsigned int i=0;i< req.ode_joint_config.erp.size();i++)
1534 joint->SetParam(
"erp",i,req.ode_joint_config.erp[i]);
1535 for(
unsigned int i=0;i< req.ode_joint_config.cfm.size();i++)
1536 joint->SetParam(
"cfm",i,req.ode_joint_config.cfm[i]);
1537 for(
unsigned int i=0;i< req.ode_joint_config.stop_erp.size();i++)
1538 joint->SetParam(
"stop_erp",i,req.ode_joint_config.stop_erp[i]);
1539 for(
unsigned int i=0;i< req.ode_joint_config.stop_cfm.size();i++)
1540 joint->SetParam(
"stop_cfm",i,req.ode_joint_config.stop_cfm[i]);
1541 for(
unsigned int i=0;i< req.ode_joint_config.fudge_factor.size();i++)
1542 joint->SetParam(
"fudge_factor",i,req.ode_joint_config.fudge_factor[i]);
1543 for(
unsigned int i=0;i< req.ode_joint_config.fmax.size();i++)
1544 joint->SetParam(
"fmax",i,req.ode_joint_config.fmax[i]);
1545 for(
unsigned int i=0;i< req.ode_joint_config.vel.size();i++)
1546 joint->SetParam(
"vel",i,req.ode_joint_config.vel[i]);
1549 res.status_message =
"SetJointProperties: properties set";
1555 gazebo_msgs::SetModelState::Response &res)
1557 ignition::math::Vector3d target_pos(req.model_state.pose.position.x,req.model_state.pose.position.y,req.model_state.pose.position.z);
1558 ignition::math::Quaterniond target_rot(req.model_state.pose.orientation.w,req.model_state.pose.orientation.x,req.model_state.pose.orientation.y,req.model_state.pose.orientation.z);
1559 target_rot.Normalize();
1560 ignition::math::Pose3d target_pose(target_pos,target_rot);
1561 ignition::math::Vector3d target_pos_dot(req.model_state.twist.linear.x,req.model_state.twist.linear.y,req.model_state.twist.linear.z);
1562 ignition::math::Vector3d target_rot_dot(req.model_state.twist.angular.x,req.model_state.twist.angular.y,req.model_state.twist.angular.z);
1564 #if GAZEBO_MAJOR_VERSION >= 8 1565 gazebo::physics::ModelPtr model =
world_->ModelByName(req.model_state.model_name);
1567 gazebo::physics::ModelPtr model =
world_->GetModel(req.model_state.model_name);
1571 ROS_ERROR_NAMED(
"api_plugin",
"Updating ModelState: model [%s] does not exist",req.model_state.model_name.c_str());
1572 res.success =
false;
1573 res.status_message =
"SetModelState: model does not exist";
1578 #if GAZEBO_MAJOR_VERSION >= 8 1579 gazebo::physics::EntityPtr relative_entity =
world_->EntityByName(req.model_state.reference_frame);
1581 gazebo::physics::EntityPtr relative_entity =
world_->GetEntity(req.model_state.reference_frame);
1583 if (relative_entity)
1585 #if GAZEBO_MAJOR_VERSION >= 8 1586 ignition::math::Pose3d frame_pose = relative_entity->WorldPose();
1588 ignition::math::Pose3d frame_pose = relative_entity->GetWorldPose().Ign();
1591 target_pose = target_pose + frame_pose;
1595 target_pos_dot = frame_pose.Rot().RotateVector(target_pos_dot);
1596 target_rot_dot = frame_pose.Rot().RotateVector(target_rot_dot);
1599 else if (req.model_state.reference_frame ==
"" || req.model_state.reference_frame ==
"world" || req.model_state.reference_frame ==
"map" || req.model_state.reference_frame ==
"/map" )
1601 ROS_DEBUG_NAMED(
"api_plugin",
"Updating ModelState: reference frame is empty/world/map, usig inertial frame");
1605 ROS_ERROR_NAMED(
"api_plugin",
"Updating ModelState: for model[%s], specified reference frame entity [%s] does not exist",
1606 req.model_state.model_name.c_str(),req.model_state.reference_frame.c_str());
1607 res.success =
false;
1608 res.status_message =
"SetModelState: specified reference frame entity does not exist";
1613 bool is_paused =
world_->IsPaused();
1615 model->SetWorldPose(target_pose);
1616 world_->SetPaused(is_paused);
1621 model->SetLinearVel(target_pos_dot);
1622 model->SetAngularVel(target_rot_dot);
1625 res.status_message =
"SetModelState: set model state done";
1632 gazebo_msgs::SetModelState::Response res;
1633 gazebo_msgs::SetModelState::Request req;
1634 req.model_state = *model_state;
1639 gazebo_msgs::ApplyJointEffort::Response &res)
1641 gazebo::physics::JointPtr joint;
1642 #if GAZEBO_MAJOR_VERSION >= 8 1643 for (
unsigned int i = 0; i <
world_->ModelCount(); i ++)
1645 joint =
world_->ModelByIndex(i)->GetJoint(req.joint_name);
1647 for (
unsigned int i = 0; i <
world_->GetModelCount(); i ++)
1649 joint =
world_->GetModel(i)->GetJoint(req.joint_name);
1655 fjj->
force = req.effort;
1657 #if GAZEBO_MAJOR_VERSION >= 8 1670 res.status_message =
"ApplyJointEffort: effort set";
1675 res.success =
false;
1676 res.status_message =
"ApplyJointEffort: joint not found";
1688 world_->ResetEntities(gazebo::physics::Base::MODEL);
1700 world_->SetPaused(
false);
1705 gazebo_msgs::JointRequest::Response &res)
1718 if ((*iter)->joint->GetName() == joint_name)
1733 gazebo_msgs::BodyRequest::Response &res)
1747 if ((*iter)->body->GetScopedName() == body_name)
1762 gazebo_msgs::SetModelConfiguration::Response &res)
1764 std::string gazebo_model_name = req.model_name;
1767 #if GAZEBO_MAJOR_VERSION >= 8 1768 gazebo::physics::ModelPtr gazebo_model =
world_->ModelByName(req.model_name);
1770 gazebo::physics::ModelPtr gazebo_model =
world_->GetModel(req.model_name);
1774 ROS_ERROR_NAMED(
"api_plugin",
"SetModelConfiguration: model [%s] does not exist",gazebo_model_name.c_str());
1775 res.success =
false;
1776 res.status_message =
"SetModelConfiguration: model does not exist";
1780 if (req.joint_names.size() == req.joint_positions.size())
1782 std::map<std::string, double> joint_position_map;
1783 for (
unsigned int i = 0; i < req.joint_names.size(); i++)
1785 joint_position_map[req.joint_names[i]] = req.joint_positions[i];
1789 bool is_paused =
world_->IsPaused();
1790 if (!is_paused)
world_->SetPaused(
true);
1792 gazebo_model->SetJointPositions(joint_position_map);
1795 world_->SetPaused(is_paused);
1798 res.status_message =
"SetModelConfiguration: success";
1803 res.success =
false;
1804 res.status_message =
"SetModelConfiguration: joint name and position list have different lengths";
1810 gazebo_msgs::SetLinkState::Response &res)
1812 #if GAZEBO_MAJOR_VERSION >= 8 1813 gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast<gazebo::physics::Link>(
world_->EntityByName(req.link_state.link_name));
1814 gazebo::physics::LinkPtr frame = boost::dynamic_pointer_cast<gazebo::physics::Link>(
world_->EntityByName(req.link_state.reference_frame));
1816 gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast<gazebo::physics::Link>(
world_->GetEntity(req.link_state.link_name));
1817 gazebo::physics::EntityPtr frame =
world_->GetEntity(req.link_state.reference_frame);
1821 ROS_ERROR_NAMED(
"api_plugin",
"Updating LinkState: link [%s] does not exist",req.link_state.link_name.c_str());
1822 res.success =
false;
1823 res.status_message =
"SetLinkState: link does not exist";
1830 ignition::math::Vector3d target_pos(req.link_state.pose.position.x,req.link_state.pose.position.y,req.link_state.pose.position.z);
1831 ignition::math::Quaterniond target_rot(req.link_state.pose.orientation.w,req.link_state.pose.orientation.x,req.link_state.pose.orientation.y,req.link_state.pose.orientation.z);
1832 ignition::math::Pose3d target_pose(target_pos,target_rot);
1833 ignition::math::Vector3d target_linear_vel(req.link_state.twist.linear.x,req.link_state.twist.linear.y,req.link_state.twist.linear.z);
1834 ignition::math::Vector3d target_angular_vel(req.link_state.twist.angular.x,req.link_state.twist.angular.y,req.link_state.twist.angular.z);
1838 #if GAZEBO_MAJOR_VERSION >= 8 1839 ignition::math::Pose3d frame_pose = frame->WorldPose();
1840 ignition::math::Vector3d frame_linear_vel = frame->WorldLinearVel();
1841 ignition::math::Vector3d frame_angular_vel = frame->WorldAngularVel();
1843 ignition::math::Pose3d frame_pose = frame->GetWorldPose().Ign();
1844 ignition::math::Vector3d frame_linear_vel = frame->GetWorldLinearVel().Ign();
1845 ignition::math::Vector3d frame_angular_vel = frame->GetWorldAngularVel().Ign();
1847 ignition::math::Vector3d frame_pos = frame_pose.Pos();
1848 ignition::math::Quaterniond frame_rot = frame_pose.Rot();
1851 target_pose = target_pose + frame_pose;
1853 target_linear_vel -= frame_linear_vel;
1854 target_angular_vel -= frame_angular_vel;
1856 else if (req.link_state.reference_frame ==
"" || req.link_state.reference_frame ==
"world" || req.link_state.reference_frame ==
"map" || req.link_state.reference_frame ==
"/map")
1858 ROS_INFO_NAMED(
"api_plugin",
"Updating LinkState: reference_frame is empty/world/map, using inertial frame");
1862 ROS_ERROR_NAMED(
"api_plugin",
"Updating LinkState: reference_frame is not a valid entity name");
1863 res.success =
false;
1864 res.status_message =
"SetLinkState: failed";
1871 bool is_paused =
world_->IsPaused();
1872 if (!is_paused)
world_->SetPaused(
true);
1873 body->SetWorldPose(target_pose);
1874 world_->SetPaused(is_paused);
1877 body->SetLinearVel(target_linear_vel);
1878 body->SetAngularVel(target_angular_vel);
1881 res.status_message =
"SetLinkState: success";
1887 gazebo_msgs::SetLinkState::Request req;
1888 gazebo_msgs::SetLinkState::Response res;
1889 req.link_state = *link_state;
1894 const ignition::math::Vector3d &reference_force,
1895 const ignition::math::Vector3d &reference_torque,
1896 const ignition::math::Pose3d &target_to_reference )
1899 target_force = target_to_reference.Rot().RotateVector(reference_force);
1901 target_torque = target_to_reference.Rot().RotateVector(reference_torque);
1904 target_torque = target_torque + target_to_reference.Pos().Cross(target_force);
1908 gazebo_msgs::ApplyBodyWrench::Response &res)
1910 #if GAZEBO_MAJOR_VERSION >= 8 1911 gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast<gazebo::physics::Link>(
world_->EntityByName(req.body_name));
1912 gazebo::physics::EntityPtr frame =
world_->EntityByName(req.reference_frame);
1914 gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast<gazebo::physics::Link>(
world_->GetEntity(req.body_name));
1915 gazebo::physics::EntityPtr frame =
world_->GetEntity(req.reference_frame);
1919 ROS_ERROR_NAMED(
"api_plugin",
"ApplyBodyWrench: body [%s] does not exist",req.body_name.c_str());
1920 res.success =
false;
1921 res.status_message =
"ApplyBodyWrench: body does not exist";
1926 ignition::math::Vector3d reference_force(req.wrench.force.x,req.wrench.force.y,req.wrench.force.z);
1927 ignition::math::Vector3d reference_torque(req.wrench.torque.x,req.wrench.torque.y,req.wrench.torque.z);
1928 ignition::math::Vector3d reference_point(req.reference_point.x,req.reference_point.y,req.reference_point.z);
1930 ignition::math::Vector3d target_force;
1931 ignition::math::Vector3d target_torque;
1935 reference_torque = reference_torque + reference_point.Cross(reference_force);
1946 #if GAZEBO_MAJOR_VERSION >= 8 1947 ignition::math::Pose3d framePose = frame->WorldPose();
1949 ignition::math::Pose3d framePose = frame->GetWorldPose().Ign();
1951 #if GAZEBO_MAJOR_VERSION >= 8 1952 ignition::math::Pose3d bodyPose = body->WorldPose();
1954 ignition::math::Pose3d bodyPose = body->GetWorldPose().Ign();
1956 ignition::math::Pose3d target_to_reference = framePose - bodyPose;
1957 ROS_DEBUG_NAMED(
"api_plugin",
"reference frame for applied wrench: [%f %f %f, %f %f %f]-[%f %f %f, %f %f %f]=[%f %f %f, %f %f %f]",
1961 bodyPose.Rot().Euler().X(),
1962 bodyPose.Rot().Euler().Y(),
1963 bodyPose.Rot().Euler().Z(),
1964 framePose.Pos().X(),
1965 framePose.Pos().Y(),
1966 framePose.Pos().Z(),
1967 framePose.Rot().Euler().X(),
1968 framePose.Rot().Euler().Y(),
1969 framePose.Rot().Euler().Z(),
1970 target_to_reference.Pos().X(),
1971 target_to_reference.Pos().Y(),
1972 target_to_reference.Pos().Z(),
1973 target_to_reference.Rot().Euler().X(),
1974 target_to_reference.Rot().Euler().Y(),
1975 target_to_reference.Rot().Euler().Z()
1977 transformWrench(target_force, target_torque, reference_force, reference_torque, target_to_reference);
1978 ROS_ERROR_NAMED(
"api_plugin",
"wrench defined as [%s]:[%f %f %f, %f %f %f] --> applied as [%s]:[%f %f %f, %f %f %f]",
1979 frame->GetName().c_str(),
1980 reference_force.X(),
1981 reference_force.Y(),
1982 reference_force.Z(),
1983 reference_torque.X(),
1984 reference_torque.Y(),
1985 reference_torque.Z(),
1986 body->GetName().c_str(),
1996 else if (req.reference_frame ==
"" || req.reference_frame ==
"world" || req.reference_frame ==
"map" || req.reference_frame ==
"/map")
1998 ROS_INFO_NAMED(
"api_plugin",
"ApplyBodyWrench: reference_frame is empty/world/map, using inertial frame, transferring from body relative to inertial frame");
2000 #if GAZEBO_MAJOR_VERSION >= 8 2001 ignition::math::Pose3d target_to_reference = body->WorldPose();
2003 ignition::math::Pose3d target_to_reference = body->GetWorldPose().Ign();
2005 target_force = reference_force;
2006 target_torque = reference_torque;
2011 ROS_ERROR_NAMED(
"api_plugin",
"ApplyBodyWrench: reference_frame is not a valid entity name");
2012 res.success =
false;
2013 res.status_message =
"ApplyBodyWrench: reference_frame not found";
2023 wej->
force = target_force;
2024 wej->
torque = target_torque;
2026 #if GAZEBO_MAJOR_VERSION >= 8 2039 res.status_message =
"";
2045 TiXmlDocument doc_in;
2046 doc_in.Parse(model_xml.c_str());
2047 if (doc_in.FirstChild(
"robot"))
2056 TiXmlDocument doc_in;
2057 doc_in.Parse(model_xml.c_str());
2058 if (doc_in.FirstChild(
"gazebo") ||
2059 doc_in.FirstChild(
"sdf"))
2073 #if GAZEBO_MAJOR_VERSION >= 8 2078 if (simTime >= (*iter)->start_time)
2079 if (simTime <= (*iter)->start_time+(*iter)->duration ||
2080 (*iter)->duration.toSec() < 0.0)
2084 (*iter)->body->SetForce((*iter)->force);
2085 (*iter)->body->SetTorque((*iter)->torque);
2088 (*iter)->duration.
fromSec(0.0);
2091 if (simTime > (*iter)->start_time+(*iter)->duration &&
2092 (*iter)->duration.toSec() >= 0.0)
2112 #if GAZEBO_MAJOR_VERSION >= 8 2117 if (simTime >= (*iter)->start_time)
2118 if (simTime <= (*iter)->start_time+(*iter)->duration ||
2119 (*iter)->duration.toSec() < 0.0)
2122 (*iter)->joint->SetForce(0,(*iter)->force);
2124 (*iter)->duration.
fromSec(0.0);
2127 if (simTime > (*iter)->start_time+(*iter)->duration &&
2128 (*iter)->duration.toSec() >= 0.0)
2142 #if GAZEBO_MAJOR_VERSION >= 8 2143 gazebo::common::Time sim_time =
world_->SimTime();
2145 gazebo::common::Time sim_time =
world_->GetSimTime();
2150 gazebo::common::Time currentTime = gazebo::msgs::Convert( msg->sim_time() );
2151 rosgraph_msgs::Clock ros_time_;
2152 ros_time_.clock.fromSec(currentTime.Double());
2159 #if GAZEBO_MAJOR_VERSION >= 8 2160 gazebo::common::Time sim_time =
world_->SimTime();
2162 gazebo::common::Time sim_time =
world_->GetSimTime();
2167 #if GAZEBO_MAJOR_VERSION >= 8 2168 gazebo::common::Time currentTime =
world_->SimTime();
2170 gazebo::common::Time currentTime =
world_->GetSimTime();
2172 rosgraph_msgs::Clock ros_time_;
2173 ros_time_.clock.fromSec(currentTime.Double());
2181 gazebo_msgs::LinkStates link_states;
2184 #if GAZEBO_MAJOR_VERSION >= 8 2185 for (
unsigned int i = 0; i <
world_->ModelCount(); i ++)
2187 gazebo::physics::ModelPtr model =
world_->ModelByIndex(i);
2189 for (
unsigned int i = 0; i <
world_->GetModelCount(); i ++)
2191 gazebo::physics::ModelPtr model =
world_->GetModel(i);
2194 for (
unsigned int j = 0 ; j < model->GetChildCount(); j ++)
2196 gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast<gazebo::physics::Link>(model->GetChild(j));
2200 link_states.name.push_back(body->GetScopedName());
2201 geometry_msgs::Pose pose;
2202 #if GAZEBO_MAJOR_VERSION >= 8 2203 ignition::math::Pose3d body_pose = body->WorldPose();
2204 ignition::math::Vector3d linear_vel = body->WorldLinearVel();
2205 ignition::math::Vector3d angular_vel = body->WorldAngularVel();
2207 ignition::math::Pose3d body_pose = body->GetWorldPose().Ign();
2208 ignition::math::Vector3d linear_vel = body->GetWorldLinearVel().Ign();
2209 ignition::math::Vector3d angular_vel = body->GetWorldAngularVel().Ign();
2211 ignition::math::Vector3d pos = body_pose.Pos();
2212 ignition::math::Quaterniond rot = body_pose.Rot();
2213 pose.position.x = pos.X();
2214 pose.position.y = pos.Y();
2215 pose.position.z = pos.Z();
2216 pose.orientation.w = rot.W();
2217 pose.orientation.x = rot.X();
2218 pose.orientation.y = rot.Y();
2219 pose.orientation.z = rot.Z();
2220 link_states.pose.push_back(pose);
2221 geometry_msgs::Twist twist;
2222 twist.linear.x = linear_vel.X();
2223 twist.linear.y = linear_vel.Y();
2224 twist.linear.z = linear_vel.Z();
2225 twist.angular.x = angular_vel.X();
2226 twist.angular.y = angular_vel.Y();
2227 twist.angular.z = angular_vel.Z();
2228 link_states.twist.push_back(twist);
2238 gazebo_msgs::ModelStates model_states;
2241 #if GAZEBO_MAJOR_VERSION >= 8 2242 for (
unsigned int i = 0; i <
world_->ModelCount(); i ++)
2244 gazebo::physics::ModelPtr model =
world_->ModelByIndex(i);
2245 ignition::math::Pose3d model_pose = model->WorldPose();
2246 ignition::math::Vector3d linear_vel = model->WorldLinearVel();
2247 ignition::math::Vector3d angular_vel = model->WorldAngularVel();
2249 for (
unsigned int i = 0; i <
world_->GetModelCount(); i ++)
2251 gazebo::physics::ModelPtr model =
world_->GetModel(i);
2252 ignition::math::Pose3d model_pose = model->GetWorldPose().Ign();
2253 ignition::math::Vector3d linear_vel = model->GetWorldLinearVel().Ign();
2254 ignition::math::Vector3d angular_vel = model->GetWorldAngularVel().Ign();
2256 ignition::math::Vector3d pos = model_pose.Pos();
2257 ignition::math::Quaterniond rot = model_pose.Rot();
2258 geometry_msgs::Pose pose;
2259 pose.position.x = pos.X();
2260 pose.position.y = pos.Y();
2261 pose.position.z = pos.Z();
2262 pose.orientation.w = rot.W();
2263 pose.orientation.x = rot.X();
2264 pose.orientation.y = rot.Y();
2265 pose.orientation.z = rot.Z();
2266 model_states.pose.push_back(pose);
2267 model_states.name.push_back(model->GetName());
2268 geometry_msgs::Twist twist;
2269 twist.linear.x = linear_vel.X();
2270 twist.linear.y = linear_vel.Y();
2271 twist.linear.z = linear_vel.Z();
2272 twist.angular.x = angular_vel.X();
2273 twist.angular.y = angular_vel.Y();
2274 twist.angular.z = angular_vel.Z();
2275 model_states.twist.push_back(twist);
2284 gazebo_msgs::GetPhysicsProperties srv;
2287 config.time_step = srv.response.time_step;
2288 config.max_update_rate = srv.response.max_update_rate;
2289 config.gravity_x = srv.response.gravity.x;
2290 config.gravity_y = srv.response.gravity.y;
2291 config.gravity_z = srv.response.gravity.z;
2292 config.auto_disable_bodies = srv.response.ode_config.auto_disable_bodies;
2293 config.sor_pgs_precon_iters = srv.response.ode_config.sor_pgs_precon_iters;
2294 config.sor_pgs_iters = srv.response.ode_config.sor_pgs_iters;
2295 config.sor_pgs_rms_error_tol = srv.response.ode_config.sor_pgs_rms_error_tol;
2296 config.sor_pgs_w = srv.response.ode_config.sor_pgs_w;
2297 config.contact_surface_layer = srv.response.ode_config.contact_surface_layer;
2298 config.contact_max_correcting_vel = srv.response.ode_config.contact_max_correcting_vel;
2299 config.cfm = srv.response.ode_config.cfm;
2300 config.erp = srv.response.ode_config.erp;
2301 config.max_contacts = srv.response.ode_config.max_contacts;
2306 bool changed =
false;
2307 gazebo_msgs::GetPhysicsProperties srv;
2311 if (config.time_step != srv.response.time_step) changed =
true;
2312 if (config.max_update_rate != srv.response.max_update_rate) changed =
true;
2313 if (config.gravity_x != srv.response.gravity.x) changed =
true;
2314 if (config.gravity_y != srv.response.gravity.y) changed =
true;
2315 if (config.gravity_z != srv.response.gravity.z) changed =
true;
2316 if (config.auto_disable_bodies != srv.response.ode_config.auto_disable_bodies) changed =
true;
2317 if ((uint32_t)config.sor_pgs_precon_iters != srv.response.ode_config.sor_pgs_precon_iters) changed =
true;
2318 if ((uint32_t)config.sor_pgs_iters != srv.response.ode_config.sor_pgs_iters) changed =
true;
2319 if (config.sor_pgs_rms_error_tol != srv.response.ode_config.sor_pgs_rms_error_tol) changed =
true;
2320 if (config.sor_pgs_w != srv.response.ode_config.sor_pgs_w) changed =
true;
2321 if (config.contact_surface_layer != srv.response.ode_config.contact_surface_layer) changed =
true;
2322 if (config.contact_max_correcting_vel != srv.response.ode_config.contact_max_correcting_vel) changed =
true;
2323 if (config.cfm != srv.response.ode_config.cfm) changed =
true;
2324 if (config.erp != srv.response.ode_config.erp) changed =
true;
2325 if ((uint32_t)config.max_contacts != srv.response.ode_config.max_contacts) changed =
true;
2330 gazebo_msgs::SetPhysicsProperties srv;
2331 srv.request.time_step = config.time_step ;
2332 srv.request.max_update_rate = config.max_update_rate ;
2333 srv.request.gravity.x = config.gravity_x ;
2334 srv.request.gravity.y = config.gravity_y ;
2335 srv.request.gravity.z = config.gravity_z ;
2336 srv.request.ode_config.auto_disable_bodies = config.auto_disable_bodies ;
2337 srv.request.ode_config.sor_pgs_precon_iters = config.sor_pgs_precon_iters ;
2338 srv.request.ode_config.sor_pgs_iters = config.sor_pgs_iters ;
2339 srv.request.ode_config.sor_pgs_rms_error_tol = config.sor_pgs_rms_error_tol ;
2340 srv.request.ode_config.sor_pgs_w = config.sor_pgs_w ;
2341 srv.request.ode_config.contact_surface_layer = config.contact_surface_layer ;
2342 srv.request.ode_config.contact_max_correcting_vel = config.contact_max_correcting_vel ;
2343 srv.request.ode_config.cfm = config.cfm ;
2344 srv.request.ode_config.erp = config.erp ;
2345 srv.request.ode_config.max_contacts = config.max_contacts ;
2347 ROS_INFO_NAMED(
"api_plugin",
"physics dynamics reconfigure update complete");
2349 ROS_INFO_NAMED(
"api_plugin",
"physics dynamics reconfigure complete");
2360 physics_reconfigure_get_client_.waitForExistence();
2367 ROS_INFO_NAMED(
"api_plugin",
"Physics dynamic reconfigure ready.");
2376 std::string open_bracket(
"<?");
2377 std::string close_bracket(
"?>");
2378 size_t pos1 = model_xml.find(open_bracket,0);
2379 size_t pos2 = model_xml.find(close_bracket,0);
2380 if (pos1 != std::string::npos && pos2 != std::string::npos)
2381 model_xml.replace(pos1,pos2-pos1+2,std::string(
""));
2385 const std::string &model_name,
2386 const ignition::math::Vector3d &initial_xyz,
2387 const ignition::math::Quaterniond &initial_q)
2392 TiXmlElement* pose_element;
2395 TiXmlElement* gazebo_tixml = gazebo_model_xml.FirstChildElement(
"sdf");
2398 ROS_WARN_NAMED(
"api_plugin",
"Could not find <sdf> element in sdf, so name and initial position cannot be applied");
2403 TiXmlElement* model_tixml = gazebo_tixml->FirstChildElement(
"model");
2407 if (model_tixml->Attribute(
"name") != NULL)
2410 model_tixml->RemoveAttribute(
"name");
2413 model_tixml->SetAttribute(
"name",model_name);
2418 TiXmlElement* world_tixml = gazebo_tixml->FirstChildElement(
"world");
2421 ROS_WARN_NAMED(
"api_plugin",
"Could not find <model> or <world> element in sdf, so name and initial position cannot be applied");
2425 model_tixml = world_tixml->FirstChildElement(
"include");
2428 ROS_WARN_NAMED(
"api_plugin",
"Could not find <include> element in sdf, so name and initial position cannot be applied");
2433 TiXmlElement* name_tixml = model_tixml->FirstChildElement(
"name");
2437 name_tixml =
new TiXmlElement(
"name");
2438 model_tixml->LinkEndChild(name_tixml);
2442 TiXmlText* text =
new TiXmlText(model_name);
2443 name_tixml->LinkEndChild( text );
2448 pose_element = model_tixml->FirstChildElement(
"pose");
2449 ignition::math::Pose3d model_pose;
2456 model_pose = this->
parsePose(pose_element->GetText());
2457 model_tixml->RemoveChild(pose_element);
2463 ignition::math::Pose3d new_model_pose = model_pose + ignition::math::Pose3d(initial_xyz, initial_q);
2466 std::ostringstream pose_stream;
2467 ignition::math::Vector3d model_rpy = new_model_pose.Rot().Euler();
2468 pose_stream << new_model_pose.Pos().X() <<
" " << new_model_pose.Pos().Y() <<
" " << new_model_pose.Pos().Z() <<
" " 2469 << model_rpy.X() <<
" " << model_rpy.Y() <<
" " << model_rpy.Z();
2472 TiXmlText* text =
new TiXmlText(pose_stream.str());
2473 TiXmlElement* new_pose_element =
new TiXmlElement(
"pose");
2474 new_pose_element->LinkEndChild(text);
2475 model_tixml->LinkEndChild(new_pose_element);
2481 std::vector<std::string> pieces;
2482 std::vector<double> vals;
2484 boost::split(pieces, str, boost::is_any_of(
" "));
2485 for (
unsigned int i = 0; i < pieces.size(); ++i)
2487 if (pieces[i] !=
"")
2491 vals.push_back(boost::lexical_cast<double>(pieces[i].c_str()));
2493 catch(boost::bad_lexical_cast &e)
2495 sdferr <<
"xml key [" << str
2496 <<
"][" << i <<
"] value [" << pieces[i]
2497 <<
"] is not a valid double from a 3-tuple\n";
2498 return ignition::math::Pose3d();
2503 if (vals.size() == 6)
2504 return ignition::math::Pose3d(vals[0], vals[1], vals[2], vals[3], vals[4], vals[5]);
2507 ROS_ERROR_NAMED(
"api_plugin",
"Beware: failed to parse string [%s] as ignition::math::Pose3d, returning zeros.", str.c_str());
2508 return ignition::math::Pose3d();
2514 std::vector<std::string> pieces;
2515 std::vector<double> vals;
2517 boost::split(pieces, str, boost::is_any_of(
" "));
2518 for (
unsigned int i = 0; i < pieces.size(); ++i)
2520 if (pieces[i] !=
"")
2524 vals.push_back(boost::lexical_cast<double>(pieces[i].c_str()));
2526 catch(boost::bad_lexical_cast &e)
2528 sdferr <<
"xml key [" << str
2529 <<
"][" << i <<
"] value [" << pieces[i]
2530 <<
"] is not a valid double from a 3-tuple\n";
2531 return ignition::math::Vector3d();
2536 if (vals.size() == 3)
2537 return ignition::math::Vector3d(vals[0], vals[1], vals[2]);
2540 ROS_ERROR_NAMED(
"api_plugin",
"Beware: failed to parse string [%s] as ignition::math::Vector3d, returning zeros.", str.c_str());
2541 return ignition::math::Vector3d();
2546 const ignition::math::Vector3d &initial_xyz,
2547 const ignition::math::Quaterniond &initial_q)
2549 TiXmlElement* model_tixml = (gazebo_model_xml.FirstChildElement(
"robot"));
2554 TiXmlElement* origin_key = model_tixml->FirstChildElement(
"origin");
2558 origin_key =
new TiXmlElement(
"origin");
2559 model_tixml->LinkEndChild(origin_key);
2562 ignition::math::Vector3d xyz;
2563 ignition::math::Vector3d rpy;
2564 if (origin_key->Attribute(
"xyz"))
2566 xyz = this->
parseVector3(origin_key->Attribute(
"xyz"));
2567 origin_key->RemoveAttribute(
"xyz");
2569 if (origin_key->Attribute(
"rpy"))
2571 rpy = this->
parseVector3(origin_key->Attribute(
"rpy"));
2572 origin_key->RemoveAttribute(
"rpy");
2576 ignition::math::Pose3d model_pose = ignition::math::Pose3d(xyz, ignition::math::Quaterniond(rpy))
2577 + ignition::math::Pose3d(initial_xyz, initial_q);
2579 std::ostringstream xyz_stream;
2580 xyz_stream << model_pose.Pos().X() <<
" " << model_pose.Pos().Y() <<
" " << model_pose.Pos().Z();
2582 std::ostringstream rpy_stream;
2583 ignition::math::Vector3d model_rpy = model_pose.Rot().Euler();
2584 rpy_stream << model_rpy.X() <<
" " << model_rpy.Y() <<
" " << model_rpy.Z();
2586 origin_key->SetAttribute(
"xyz",xyz_stream.str());
2587 origin_key->SetAttribute(
"rpy",rpy_stream.str());
2590 ROS_WARN_NAMED(
"api_plugin",
"Could not find <model> element in sdf, so name and initial position is not applied");
2595 TiXmlElement* model_tixml = gazebo_model_xml.FirstChildElement(
"robot");
2599 if (model_tixml->Attribute(
"name") != NULL)
2602 model_tixml->RemoveAttribute(
"name");
2605 model_tixml->SetAttribute(
"name",model_name);
2608 ROS_WARN_NAMED(
"api_plugin",
"Could not find <robot> element in URDF, name not replaced");
2613 TiXmlNode* child = 0;
2614 child = model_xml->IterateChildren(child);
2615 while (child != NULL)
2617 if (child->Type() == TiXmlNode::TINYXML_ELEMENT &&
2618 child->ValueStr().compare(std::string(
"plugin")) == 0)
2620 if (child->FirstChildElement(
"robotNamespace") == NULL)
2622 TiXmlElement* child_elem = child->ToElement()->FirstChildElement(
"robotNamespace");
2625 child->ToElement()->RemoveChild(child_elem);
2626 child_elem = child->ToElement()->FirstChildElement(
"robotNamespace");
2628 TiXmlElement* key =
new TiXmlElement(
"robotNamespace");
2630 key->LinkEndChild(val);
2631 child->ToElement()->LinkEndChild(key);
2635 child = model_xml->IterateChildren(child);
2640 gazebo_msgs::SpawnModel::Response &res)
2642 std::string entity_type = gazebo_model_xml.RootElement()->FirstChild()->Value();
2644 std::transform(entity_type.begin(), entity_type.end(), entity_type.begin(), ::tolower);
2646 bool isLight = (entity_type ==
"light");
2649 std::ostringstream stream;
2650 stream << gazebo_model_xml;
2651 std::string gazebo_model_xml_string = stream.str();
2652 ROS_DEBUG_NAMED(
"api_plugin.xml",
"Gazebo Model XML\n\n%s\n\n ",gazebo_model_xml_string.c_str());
2655 gazebo::msgs::Factory msg;
2656 gazebo::msgs::Init(msg,
"spawn_model");
2657 msg.set_sdf( gazebo_model_xml_string );
2663 gazebo::msgs::Request *entity_info_msg = gazebo::msgs::CreateRequest(
"entity_info", model_name);
2665 delete entity_info_msg;
2666 entity_info_msg =
nullptr;
2669 #if GAZEBO_MAJOR_VERSION >= 8 2670 gazebo::physics::ModelPtr model =
world_->ModelByName(model_name);
2671 gazebo::physics::LightPtr light =
world_->LightByName(model_name);
2673 gazebo::physics::ModelPtr model =
world_->GetModel(model_name);
2674 gazebo::physics::LightPtr light =
world_->Light(model_name);
2676 if ((isLight && light != NULL) || (model != NULL))
2678 ROS_ERROR_NAMED(
"api_plugin",
"SpawnModel: Failure - model name %s already exists.",model_name.c_str());
2679 res.success =
false;
2680 res.status_message =
"SpawnModel: Failure - entity already exists.";
2689 sdf_light.SetFromString(gazebo_model_xml_string);
2690 gazebo::msgs::Light msg = gazebo::msgs::LightFromSDF(sdf_light.Root()->GetElement(
"light"));
2691 msg.set_name(model_name);
2710 res.success =
false;
2711 res.status_message =
"SpawnModel: Entity pushed to spawn queue, but spawn service timed out waiting for entity to appear in simulation under the name " + model_name;
2717 #if GAZEBO_MAJOR_VERSION >= 8 2718 if ((isLight &&
world_->LightByName(model_name) != NULL)
2719 || (
world_->ModelByName(model_name) != NULL))
2721 if ((isLight &&
world_->Light(model_name) != NULL)
2722 || (
world_->GetModel(model_name) != NULL))
2728 <<
" for entity " << model_name <<
" to spawn");
2735 res.status_message =
"SpawnModel: Successfully spawned entity";
bool getLinkProperties(gazebo_msgs::GetLinkProperties::Request &req, gazebo_msgs::GetLinkProperties::Response &res)
void shutdownSignal()
Detect if sig-int shutdown signal is recieved
#define ROS_DEBUG_ONCE_NAMED(name,...)
ros::ServiceServer reset_simulation_service_
ignition::math::Vector3d torque
bool getPhysicsProperties(gazebo_msgs::GetPhysicsProperties::Request &req, gazebo_msgs::GetPhysicsProperties::Response &res)
gazebo::physics::LinkPtr body
ros::ServiceServer apply_body_wrench_service_
bool setJointProperties(gazebo_msgs::SetJointProperties::Request &req, gazebo_msgs::SetJointProperties::Response &res)
void updateURDFModelPose(TiXmlDocument &gazebo_model_xml, const ignition::math::Vector3d &initial_xyz, const ignition::math::Quaterniond &initial_q)
Update the model pose of the URDF file before sending to Gazebo.
bool setPhysicsProperties(gazebo_msgs::SetPhysicsProperties::Request &req, gazebo_msgs::SetPhysicsProperties::Response &res)
gazebo::event::ConnectionPtr time_update_event_
bool setModelConfiguration(gazebo_msgs::SetModelConfiguration::Request &req, gazebo_msgs::SetModelConfiguration::Response &res)
bool getModelState(gazebo_msgs::GetModelState::Request &req, gazebo_msgs::GetModelState::Response &res)
ros::ServiceServer set_physics_properties_service_
void onLinkStatesDisconnect()
Callback for a subscriber disconnecting from LinkStates ros topic.
ros::Publisher pub_performance_metrics_
ros::ServiceServer get_model_properties_service_
#define ROS_INFO_NAMED(name,...)
ros::ServiceServer unpause_physics_service_
#define ROS_DEBUG_STREAM_NAMED(name, args)
void forceJointSchedulerSlot()
bool setLinkProperties(gazebo_msgs::SetLinkProperties::Request &req, gazebo_msgs::SetLinkProperties::Response &res)
boost::shared_ptr< dynamic_reconfigure::Server< gazebo_ros::PhysicsConfig > > physics_reconfigure_srv_
void publish(const boost::shared_ptr< M > &message) const
#define ROS_WARN_NAMED(name,...)
void loadGazeboRosApiPlugin(std::string world_name)
Connect to Gazebo via its plugin interface, get a pointer to the world, start events.
gazebo::common::Time last_pub_clock_time_
ros::ServiceServer spawn_urdf_model_service_
ros::ServiceServer get_light_properties_service_
dynamic_reconfigure::Server< gazebo_ros::PhysicsConfig >::CallbackType physics_reconfigure_callback_
bool isURDF(std::string model_xml)
utility for checking if string is in URDF format
boost::shared_ptr< ros::NodeHandle > nh_
void onLinkStatesConnect()
Callback for a subscriber connecting to LinkStates ros topic.
void wrenchBodySchedulerSlot()
boost::shared_ptr< boost::thread > physics_reconfigure_thread_
ROSCPP_DECL bool isInitialized()
void physicsReconfigureThread()
waits for the rest of Gazebo to be ready before initializing the dynamic reconfigure services ...
bool setLinkState(gazebo_msgs::SetLinkState::Request &req, gazebo_msgs::SetLinkState::Response &res)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
bool call(MReq &req, MRes &res)
gazebo::transport::PublisherPtr request_pub_
ros::ServiceServer clear_body_wrenches_service_
ros::Subscriber set_link_state_topic_
ignition::math::Pose3d parsePose(const std::string &str)
convert xml to Pose
void onModelStatesConnect()
Callback for a subscriber connecting to ModelStates ros topic.
ros::ServiceServer get_link_properties_service_
ros::ServiceServer set_model_configuration_service_
void advertiseServices()
advertise services
gazebo::physics::WorldPtr world_
int pub_model_states_connection_count_
ros::ServiceClient physics_reconfigure_set_client_
ros::ServiceServer delete_light_service_
bool resetSimulation(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
ros::ServiceServer delete_model_service_
bool unpausePhysics(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
ros::ServiceServer apply_joint_effort_service_
ros::ServiceServer clear_joint_forces_service_
std::vector< GazeboRosApiPlugin::ForceJointJob * > force_joint_jobs_
std::map< std::string, unsigned int > access_count_get_model_state_
index counters to count the accesses on models via GetModelState
#define ROS_DEBUG_NAMED(name,...)
boost::mutex lock_
A mutex to lock access to fields that are used in ROS message callbacks.
void transformWrench(ignition::math::Vector3d &target_force, ignition::math::Vector3d &target_torque, const ignition::math::Vector3d &reference_force, const ignition::math::Vector3d &reference_torque, const ignition::math::Pose3d &target_to_reference)
helper function for applyBodyWrench shift wrench from reference frame to target frame ...
ros::ServiceServer set_model_state_service_
ros::ServiceClient physics_reconfigure_get_client_
ros::ServiceServer reset_world_service_
gazebo::physics::JointPtr joint
bool applyJointEffort(gazebo_msgs::ApplyJointEffort::Request &req, gazebo_msgs::ApplyJointEffort::Response &res)
void updateSDFAttributes(TiXmlDocument &gazebo_model_xml, const std::string &model_name, const ignition::math::Vector3d &initial_xyz, const ignition::math::Quaterniond &initial_q)
Update the model name and pose of the SDF file before sending to Gazebo.
bool getJointProperties(gazebo_msgs::GetJointProperties::Request &req, gazebo_msgs::GetJointProperties::Response &res)
ros::ServiceServer get_joint_properties_service_
bool getWorldProperties(gazebo_msgs::GetWorldProperties::Request &req, gazebo_msgs::GetWorldProperties::Response &res)
void updateModelState(const gazebo_msgs::ModelState::ConstPtr &model_state)
void publishModelStates()
gazebo::event::ConnectionPtr sigint_event_
bool waitForExistence(ros::Duration timeout=ros::Duration(-1))
ros::Publisher pub_clock_
gazebo::event::ConnectionPtr pub_model_states_event_
bool deleteLight(gazebo_msgs::DeleteLight::Request &req, gazebo_msgs::DeleteLight::Response &res)
delete a given light by name
gazebo::event::ConnectionPtr wrench_update_event_
void Load(int argc, char **argv)
Gazebo-inherited load function.
gazebo::transport::PublisherPtr factory_light_pub_
bool pausePhysics(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
ros::ServiceServer set_link_properties_service_
ros::ServiceServer set_light_properties_service_
int pub_link_states_connection_count_
bool clearBodyWrenches(gazebo_msgs::BodyRequest::Request &req, gazebo_msgs::BodyRequest::Response &res)
gazebo::transport::SubscriberPtr response_sub_
int pub_performance_metrics_connection_count_
GazeboRosApiPlugin()
Constructor.
bool getModelProperties(gazebo_msgs::GetModelProperties::Request &req, gazebo_msgs::GetModelProperties::Response &res)
boost::shared_ptr< boost::thread > gazebo_callback_queue_thread_
ros::CallbackQueue gazebo_queue_
gazebo::event::ConnectionPtr force_update_event_
bool spawnURDFModel(gazebo_msgs::SpawnModel::Request &req, gazebo_msgs::SpawnModel::Response &res)
Function for inserting a URDF into Gazebo from ROS Service Call.
void walkChildAddRobotNamespace(TiXmlNode *model_xml)
std::string robot_namespace_
bool resetWorld(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
#define ROS_DEBUG_STREAM_ONCE_NAMED(name, args)
bool isSDF(std::string model_xml)
utility for checking if string is in SDF format
ROSLIB_DECL std::string getPath(const std::string &package_name)
gazebo::transport::SubscriberPtr performance_metric_sub_
boost::shared_ptr< ros::AsyncSpinner > async_ros_spin_
A plugin loaded within the gzserver on startup.
bool applyBodyWrench(gazebo_msgs::ApplyBodyWrench::Request &req, gazebo_msgs::ApplyBodyWrench::Response &res)
ros::ServiceServer get_model_state_service_
void onModelStatesDisconnect()
Callback for a subscriber disconnecting from ModelStates ros topic.
void physicsReconfigureCallback(gazebo_ros::PhysicsConfig &config, uint32_t level)
Used for the dynamic reconfigure callback function template.
bool clearJointForces(gazebo_msgs::JointRequest::Request &req, gazebo_msgs::JointRequest::Response &res)
ros::ServiceServer set_joint_properties_service_
gazebo::transport::PublisherPtr factory_pub_
gazebo::event::ConnectionPtr load_gazebo_ros_api_plugin_event_
bool spawnAndConform(TiXmlDocument &gazebo_model_xml, const std::string &model_name, gazebo_msgs::SpawnModel::Response &res)
ros::ServiceServer spawn_sdf_model_service_
ignition::math::Vector3d parseVector3(const std::string &str)
convert xml to Pose
bool getLightProperties(gazebo_msgs::GetLightProperties::Request &req, gazebo_msgs::GetLightProperties::Response &res)
ros::ServiceServer get_world_properties_service_
ros::ServiceServer set_link_state_service_
std::vector< GazeboRosApiPlugin::WrenchBodyJob * > wrench_body_jobs_
gazebo::event::ConnectionPtr pub_link_states_event_
#define ROS_FATAL_NAMED(name,...)
#define ROS_ERROR_NAMED(name,...)
ros::ServiceServer get_link_state_service_
void updateLinkState(const gazebo_msgs::LinkState::ConstPtr &link_state)
gazebo::transport::NodePtr gazebonode_
void gazeboQueueThread()
ros queue thread for this node
bool setLightProperties(gazebo_msgs::SetLightProperties::Request &req, gazebo_msgs::SetLightProperties::Response &res)
bool deleteModel(gazebo_msgs::DeleteModel::Request &req, gazebo_msgs::DeleteModel::Response &res)
delete model given name
bool spawnSDFModel(gazebo_msgs::SpawnModel::Request &req, gazebo_msgs::SpawnModel::Response &res)
Both SDFs and converted URDFs get sent to this function for further manipulation from a ROS Service c...
ros::Publisher pub_link_states_
ros::Subscriber set_model_state_topic_
void onResponse(ConstResponsePtr &response)
Unused.
void stripXmlDeclaration(std::string &model_xml)
ROSCPP_DECL bool search(const std::string &ns, const std::string &key, std::string &result)
ros::ServiceServer pause_physics_service_
boost::shared_ptr< void > VoidPtr
bool getLinkState(gazebo_msgs::GetLinkState::Request &req, gazebo_msgs::GetLinkState::Response &res)
void updateURDFName(TiXmlDocument &gazebo_model_xml, const std::string &model_name)
Update the model name of the URDF file before sending to Gazebo.
~GazeboRosApiPlugin()
Destructor.
ros::ServiceServer get_physics_properties_service_
ros::Publisher pub_model_states_
ignition::math::Vector3d force
bool setModelState(gazebo_msgs::SetModelState::Request &req, gazebo_msgs::SetModelState::Response &res)
gazebo::transport::PublisherPtr light_modify_pub_
#define ROS_WARN_STREAM_NAMED(name, args)
bool physics_reconfigure_initialized_