Skip to content

Commit

Permalink
Added demo_gazebo.launch (#1051) (#1231)
Browse files Browse the repository at this point in the history 
* add transmission elements to the urdf to simulate controllers using gazebo_ros_control

* add demo_gazebo to launch gazebo at the same time as rviz

* add getjointhradwareinterface function

*  add gazebo_ros as an optional dep in SA

* move sensors_3d.yaml to templates dir in SA, add comments, replace  size_t by std::size_t
  • Loading branch information
@davetcoleman
davetcoleman authored Dec 3, 2018
1 parent 7b7a8e8 commit c990011
Show file tree
Hide file tree
Showing 14 changed files with 275 additions and 88 deletions.
10 changes: 8 additions & 2 deletions moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -324,6 +324,12 @@ class MoveItConfigData
bool outputROSControllersYAML(const std::string& file_path);
bool output3DSensorPluginYAML(const std::string& file_path);

/**
* \brief Helper function to get the controller that is controlling the joint
* \return controller type
*/
std::string getJointHardwareInterface(const std::string& joint_name);

/**
* \brief Parses the existing urdf and constructs a string from it with the elements required by gazebo simulator
* added
Expand Down Expand Up @@ -368,10 +374,10 @@ class MoveItConfigData

/**
* Helper function for parsing ros_controllers.yaml file
* @param YAML::Node - controllers to be parsed
* @param std::ifstream of ros_controller.yaml
* @return true if the file was read correctly
*/
bool processROSControllers(const YAML::Node& controllers);
bool processROSControllers(std::ifstream& input_stream);

/**
* Input ros_controllers.yaml file for editing its values
Expand Down
2 changes: 2 additions & 0 deletions moveit_setup_assistant/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,8 @@
<run_depend>yaml-cpp</run_depend>
<run_depend>xacro</run_depend>
<run_depend version_gt="0.3.1">srdfdom</run_depend>
<!-- Optional: Gazebo integration. This is a heavy dependency so is disabled by default -->
<!-- run_depend>gazebo_ros</run_depend -->

<test_depend>moveit_resources</test_depend>
</package>
135 changes: 98 additions & 37 deletions moveit_setup_assistant/src/tools/moveit_config_data.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -375,6 +375,30 @@ bool MoveItConfigData::outputKinematicsYAML(const std::string& file_path)
return true; // file created successfully
}

// ******************************************************************************************
// Helper function to get the controller that is controlling the joint
// ******************************************************************************************
std::string MoveItConfigData::getJointHardwareInterface(const std::string& joint_name)
{
for (std::size_t i = 0; i < ros_controllers_config_.size(); ++i)
{
std::vector<std::string>::iterator joint_it =
std::find(ros_controllers_config_[i].joints_.begin(), ros_controllers_config_[i].joints_.end(), joint_name);
if (joint_it != ros_controllers_config_[i].joints_.end())
{
if (ros_controllers_config_[i].type_.substr(0, 8) == "position")
return "hardware_interface/PositionJointInterface";
else if (ros_controllers_config_[i].type_.substr(0, 8) == "velocity")
return "hardware_interface/VelocityJointInterface";
// As of writing this, available joint command interfaces are position, velocity and effort.
else
return "hardware_interface/EffortJointInterface";
}
}
// If the joint was not found in any controller return EffortJointInterface
return "hardware_interface/EffortJointInterface";
}

// ******************************************************************************************
// Writes a Gazebo compatible robot URDF to gazebo_compatible_urdf_string_
// ******************************************************************************************
Expand All @@ -388,34 +412,82 @@ std::string MoveItConfigData::getGazeboCompatibleURDF()
urdf_document.Parse((const char*)urdf_string_.c_str(), 0, TIXML_ENCODING_UTF8);
try
{
for (TiXmlElement* link_element = urdf_document.RootElement()->FirstChildElement(); link_element != NULL;
link_element = link_element->NextSiblingElement())
for (TiXmlElement* doc_element = urdf_document.RootElement()->FirstChildElement(); doc_element != NULL;
doc_element = doc_element->NextSiblingElement())
{
if (((std::string)link_element->Value()).find("link") == std::string::npos)
continue;
// Before adding inertial elements, make sure there is none and the link has collision element
if (link_element->FirstChildElement("inertial") == NULL && link_element->FirstChildElement("collision") != NULL)
if (static_cast<std::string>(doc_element->Value()).find("link") != std::string::npos)
{
new_urdf_needed = true;
TiXmlElement inertia_link("inertial");
TiXmlElement mass("mass");
TiXmlElement inertia_joint("inertia");
// Before adding inertial elements, make sure there is none and the link has collision element
if (doc_element->FirstChildElement("inertial") == NULL && doc_element->FirstChildElement("collision") != NULL)
{
new_urdf_needed = true;
TiXmlElement inertia_link("inertial");
TiXmlElement mass("mass");
TiXmlElement inertia_joint("inertia");

mass.SetAttribute("value", "0.1");
mass.SetAttribute("value", "0.1");

inertia_joint.SetAttribute("ixx", "0.03");
inertia_joint.SetAttribute("iyy", "0.03");
inertia_joint.SetAttribute("izz", "0.03");
inertia_joint.SetAttribute("ixy", "0.0");
inertia_joint.SetAttribute("ixz", "0.0");
inertia_joint.SetAttribute("iyz", "0.0");
inertia_joint.SetAttribute("ixx", "0.03");
inertia_joint.SetAttribute("iyy", "0.03");
inertia_joint.SetAttribute("izz", "0.03");
inertia_joint.SetAttribute("ixy", "0.0");
inertia_joint.SetAttribute("ixz", "0.0");
inertia_joint.SetAttribute("iyz", "0.0");

inertia_link.InsertEndChild(mass);
inertia_link.InsertEndChild(inertia_joint);
inertia_link.InsertEndChild(mass);
inertia_link.InsertEndChild(inertia_joint);

link_element->InsertEndChild(inertia_link);
doc_element->InsertEndChild(inertia_link);
}
}
else if (static_cast<std::string>(doc_element->Value()).find("joint") != std::string::npos)
{
// Before adding a transmission element, make sure there the joint is not fixed
if (static_cast<std::string>(doc_element->Attribute("type")) != "fixed")
{
new_urdf_needed = true;
std::string joint_name = static_cast<std::string>(doc_element->Attribute("name"));
TiXmlElement transmission("transmission");
TiXmlElement type("type");
TiXmlElement joint("joint");
TiXmlElement hardwareInterface("hardwareInterface");
TiXmlElement actuator("actuator");
TiXmlElement mechanical_reduction("mechanicalReduction");

transmission.SetAttribute("name", std::string("trans_") + joint_name);
joint.SetAttribute("name", joint_name);
actuator.SetAttribute("name", joint_name + std::string("_motor"));

type.InsertEndChild(TiXmlText("transmission_interface/SimpleTransmission"));
transmission.InsertEndChild(type);

hardwareInterface.InsertEndChild(TiXmlText(getJointHardwareInterface(joint_name).c_str()));
joint.InsertEndChild(hardwareInterface);
transmission.InsertEndChild(joint);

mechanical_reduction.InsertEndChild(TiXmlText("1"));
actuator.InsertEndChild(hardwareInterface);
actuator.InsertEndChild(mechanical_reduction);
transmission.InsertEndChild(actuator);

urdf_document.RootElement()->InsertEndChild(transmission);
}
}
}

// Add gazebo_ros_control plugin which reads the transmission tags
TiXmlElement gazebo("gazebo");
TiXmlElement plugin("plugin");
TiXmlElement robot_namespace("robotNamespace");

plugin.SetAttribute("name", "gazebo_ros_control");
plugin.SetAttribute("filename", "libgazebo_ros_control.so");
robot_namespace.InsertEndChild(TiXmlText(std::string("/")));

plugin.InsertEndChild(robot_namespace);
gazebo.InsertEndChild(plugin);

urdf_document.RootElement()->InsertEndChild(gazebo);
}
catch (YAML::ParserException& e) // Catch errors
{
Expand Down Expand Up @@ -761,10 +833,6 @@ bool MoveItConfigData::outputROSControllersYAML(const std::string& file_path)
YAML::Emitter emitter;
emitter << YAML::BeginMap;

// Start with the robot name ---------------------------------------------------
emitter << YAML::Key << srdf_->srdf_model_->getName();
emitter << YAML::Value << YAML::BeginMap;

{
emitter << YAML::Comment("MoveIt-specific simulation settings");
emitter << YAML::Key << "moveit_sim_hw_interface" << YAML::Value << YAML::BeginMap;
Expand Down Expand Up @@ -893,7 +961,6 @@ bool MoveItConfigData::outputROSControllersYAML(const std::string& file_path)
emitter << YAML::EndMap;
}
}
emitter << YAML::EndMap;

std::ofstream output_stream(file_path.c_str(), std::ios_base::trunc);
if (!output_stream.good())
Expand Down Expand Up @@ -1277,10 +1344,11 @@ bool MoveItConfigData::parseROSController(const YAML::Node& controller)
// ******************************************************************************************
// Helper function for parsing ROSControllers from ros_controllers yaml file
// ******************************************************************************************
bool MoveItConfigData::processROSControllers(const YAML::Node& controllers)
bool MoveItConfigData::processROSControllers(std::ifstream& input_stream)
{
// Used in parsing ROS controllers
ROSControlConfig control_setting;
YAML::Node controllers = YAML::Load(input_stream);

// Loop through all controllers
for (YAML::const_iterator controller_it = controllers.begin(); controller_it != controllers.end(); ++controller_it)
Expand Down Expand Up @@ -1338,28 +1406,22 @@ bool MoveItConfigData::inputROSControllersYAML(const std::string& file_path)
std::ifstream input_stream(file_path.c_str());
if (!input_stream.good())
{
ROS_WARN_STREAM_NAMED("ros_controller.yaml", "Does not exist " << file_path);
ROS_WARN_STREAM_NAMED("ros_controllers.yaml", "Does not exist " << file_path);
return false;
}

// Begin parsing
try
{
YAML::Node doc = YAML::Load(input_stream);

for (YAML::const_iterator doc_map_it = doc.begin(); doc_map_it != doc.end(); ++doc_map_it)
{
if (const YAML::Node& controllers = doc_map_it->second)
processROSControllers(controllers);
}
processROSControllers(input_stream);
}
catch (YAML::ParserException& e) // Catch errors
{
ROS_ERROR_STREAM(e.what());
return false;
}

return true; // file created successfully
return true; // file read successfully
}

// ******************************************************************************************
Expand Down Expand Up @@ -1603,7 +1665,6 @@ bool MoveItConfigData::input3DSensorsYAML(const std::string& default_file_path,
try
{
const YAML::Node& doc = YAML::Load(input_stream);

// Get sensors node
const YAML::Node& sensors_node = doc["sensors"];

Expand Down Expand Up @@ -1768,7 +1829,7 @@ std::vector<std::map<std::string, GenericParameter>> MoveItConfigData::getSensor
// ******************************************************************************************
void MoveItConfigData::clearSensorPluginConfig()
{
for (size_t param_id = 0; param_id < sensors_plugin_config_parameter_list_.size(); ++param_id)
for (std::size_t param_id = 0; param_id < sensors_plugin_config_parameter_list_.size(); ++param_id)
{
sensors_plugin_config_parameter_list_[param_id].clear();
}
Expand Down
22 changes: 21 additions & 1 deletion moveit_setup_assistant/src/widgets/configuration_files_widget.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -488,6 +488,24 @@ bool ConfigurationFilesWidget::loadGenFiles()
file.write_on_changes = 0;
gen_files_.push_back(file);

// gazebo.launch ------------------------------------------------------------------
file.file_name_ = "gazebo.launch";
file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_);
template_path = config_data_->appendPaths(template_launch_path, "gazebo.launch");
file.description_ = "Gazebo launch file which also launches ros_controllers and sends robot urdf to param server, "
"then using gazebo_ros pkg the robot is spawned to Gazebo";
file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1);
gen_files_.push_back(file);

// demo_gazebo.launch ------------------------------------------------------------------
file.file_name_ = "demo_gazebo.launch";
file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_);
template_path = config_data_->appendPaths(template_launch_path, file.file_name_);
file.description_ = "Run a demo of MoveIt with Gazebo and Rviz";
file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1);
file.write_on_changes = 0;
gen_files_.push_back(file);

// joystick_control.launch ------------------------------------------------------------------
file.file_name_ = "joystick_control.launch";
file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_);
Expand Down Expand Up @@ -1097,7 +1115,9 @@ void ConfigurationFilesWidget::loadTemplateStrings()
for (std::vector<ROSControlConfig>::iterator controller_it = config_data_->getROSControllers().begin();
controller_it != config_data_->getROSControllers().end(); ++controller_it)
{
controllers << controller_it->name_ << " ";
// Check if the controller belongs to controller_list namespace
if (controller_it->type_ != "FollowJointTrajectory")
controllers << controller_it->name_ << " ";
}
addTemplateString("[ROS_CONTROLLERS]", controllers.str());
}
Expand Down
2 changes: 1 addition & 1 deletion moveit_setup_assistant/src/widgets/kinematic_chain_widget.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -182,7 +182,7 @@ void KinematicChainWidget::addLinktoTreeRecursive(const robot_model::LinkModel*
}
}
}
for (size_t i = 0; i < link->getChildJointModels().size(); i++)
for (std::size_t i = 0; i < link->getChildJointModels().size(); i++)
{
addLinktoTreeRecursive(link->getChildJointModels()[i]->getChildLinkModel(), link);
}
Expand Down
32 changes: 16 additions & 16 deletions moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -131,9 +131,9 @@ SetupAssistantWidget::SetupAssistantWidget(QWidget* parent, boost::program_optio
nav_name_list_ << "Robot Poses";
nav_name_list_ << "End Effectors";
nav_name_list_ << "Passive Joints";
nav_name_list_ << "3D Perception";
nav_name_list_ << "Simulation";
nav_name_list_ << "ROS Control";
nav_name_list_ << "Simulation";
nav_name_list_ << "3D Perception";
nav_name_list_ << "Author Information";
nav_name_list_ << "Configuration Files";

Expand Down Expand Up @@ -303,15 +303,15 @@ void SetupAssistantWidget::progressPastStartScreen()
SLOT(highlightGroup(const std::string&)));
connect(passive_joints_widget_, SIGNAL(unhighlightAll()), this, SLOT(unhighlightAll()));

// Perception
perception_widget_ = new PerceptionWidget(this, config_data_);
main_content_->addWidget(perception_widget_);
connect(perception_widget_, SIGNAL(isModal(bool)), this, SLOT(setModalMode(bool)));
connect(perception_widget_, SIGNAL(highlightLink(const std::string&, const QColor&)), this,
// ROS Controllers
controllers_widget_ = new moveit_ros_control::ROSControllersWidget(this, config_data_);
main_content_->addWidget(controllers_widget_);
connect(controllers_widget_, SIGNAL(isModal(bool)), this, SLOT(setModalMode(bool)));
connect(controllers_widget_, SIGNAL(highlightLink(const std::string&, const QColor&)), this,
SLOT(highlightLink(const std::string&, const QColor&)));
connect(perception_widget_, SIGNAL(highlightGroup(const std::string&)), this,
connect(controllers_widget_, SIGNAL(highlightGroup(const std::string&)), this,
SLOT(highlightGroup(const std::string&)));
connect(perception_widget_, SIGNAL(unhighlightAll()), this, SLOT(unhighlightAll()));
connect(controllers_widget_, SIGNAL(unhighlightAll()), this, SLOT(unhighlightAll()));

// Simulation Screen
simulation_widget_ = new SimulationWidget(this, config_data_);
Expand All @@ -323,15 +323,15 @@ void SetupAssistantWidget::progressPastStartScreen()
SLOT(highlightGroup(const std::string&)));
connect(simulation_widget_, SIGNAL(unhighlightAll()), this, SLOT(unhighlightAll()));

// ROS Controllers
controllers_widget_ = new moveit_ros_control::ROSControllersWidget(this, config_data_);
main_content_->addWidget(controllers_widget_);
connect(controllers_widget_, SIGNAL(isModal(bool)), this, SLOT(setModalMode(bool)));
connect(controllers_widget_, SIGNAL(highlightLink(const std::string&, const QColor&)), this,
// Perception
perception_widget_ = new PerceptionWidget(this, config_data_);
main_content_->addWidget(perception_widget_);
connect(perception_widget_, SIGNAL(isModal(bool)), this, SLOT(setModalMode(bool)));
connect(perception_widget_, SIGNAL(highlightLink(const std::string&, const QColor&)), this,
SLOT(highlightLink(const std::string&, const QColor&)));
connect(controllers_widget_, SIGNAL(highlightGroup(const std::string&)), this,
connect(perception_widget_, SIGNAL(highlightGroup(const std::string&)), this,
SLOT(highlightGroup(const std::string&)));
connect(controllers_widget_, SIGNAL(unhighlightAll()), this, SLOT(unhighlightAll()));
connect(perception_widget_, SIGNAL(unhighlightAll()), this, SLOT(unhighlightAll()));

// Author Information
author_information_widget_ = new AuthorInformationWidget(this, config_data_);
Expand Down
Loading

0 comments on commit c990011

Please sign in to comment.