Skip to content

Support ros namespaces for multiple robots #106

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
iamtesch merged 6 commits into from
Nov 19, 2018
Merged

Support ros namespaces for multiple robots #106

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
9bf407e
Change hebeiros_gazebo_plugin ROS names from global to local
Nov 2, 2018
99f93bf
Get robotNamespace element that gazebo_ros spawn_model adds to XML to…
Nov 2, 2018
061585a
Change hebeiros_node ROS names from global to local
Nov 2, 2018
af66178
Remove ROS_INFO() statements used to debug
Nov 2, 2018
6bfaf17
Remove extra line already included in another commit
Nov 2, 2018
b9a56b9
Readd missing add_group_srv
Nov 19, 2018
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 3 additions & 3 deletions hebiros/src/hebiros.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,12 +29,12 @@ HebirosNode::HebirosNode (int argc, char **argv) {
}

if (use_gazebo) {
HebirosParameters::setBool("/use_sim_time", true);
HebirosParameters::setBool("use_sim_time", true);
services_gazebo.registerNodeServices();
clients.registerNodeClients();
}
else {
HebirosParameters::setBool("/use_sim_time", false);
HebirosParameters::setBool("use_sim_time", false);
services_physical.registerNodeServices();
}

Expand All @@ -47,7 +47,7 @@ void HebirosNode::cleanup() {
}

void HebirosNode::loop() {
ros::Rate loop_rate(HebirosParameters::getInt("/hebiros/node_frequency"));
ros::Rate loop_rate(HebirosParameters::getInt("hebiros/node_frequency"));

while(ros::ok()) {
ros::spinOnce();
Expand Down
4 changes: 2 additions & 2 deletions hebiros/src/hebiros_actions.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@ void HebirosActions::registerGroupActions(std::string group_name) {

trajectory_actions[group_name] = std::make_shared<
actionlib::SimpleActionServer<TrajectoryAction>>(
*HebirosNode::n_ptr, "/hebiros/"+group_name+"/trajectory",
*HebirosNode::n_ptr, "hebiros/"+group_name+"/trajectory",
boost::bind(&HebirosActions::trajectory, this, _1, group_name), false);

trajectory_actions[group_name]->start();
Expand Down Expand Up @@ -75,7 +75,7 @@ void HebirosActions::trajectory(const TrajectoryGoalConstPtr& goal, std::string
double loop_duration;
TrajectoryFeedback feedback;

ros::Rate loop_rate(HebirosParameters::getInt("/hebiros/action_frequency"));
ros::Rate loop_rate(HebirosParameters::getInt("hebiros/action_frequency"));

ROS_INFO("Group [%s]: Executing trajectory", group_name.c_str());
previous_time = ros::Time::now().toSec();
Expand Down
24 changes: 12 additions & 12 deletions hebiros/src/hebiros_clients.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,52 +9,52 @@ std::map<std::string, ros::ServiceClient> HebirosClients::clients;

void HebirosClients::registerNodeClients() {

clients["/hebiros_gazebo_plugin/add_group"] =
HebirosNode::n_ptr->serviceClient<AddGroupFromNamesSrv>("/hebiros_gazebo_plugin/add_group");
clients["hebiros_gazebo_plugin/add_group"] =
HebirosNode::n_ptr->serviceClient<AddGroupFromNamesSrv>("hebiros_gazebo_plugin/add_group");
}

void HebirosClients::registerGroupClients(std::string group_name) {

clients["/hebiros_gazebo_plugin/set_command_lifetime/"+group_name] =
clients["hebiros_gazebo_plugin/set_command_lifetime/"+group_name] =
HebirosNode::n_ptr->serviceClient<SetCommandLifetimeSrv>(
"/hebiros_gazebo_plugin/set_command_lifetime/"+group_name);
"hebiros_gazebo_plugin/set_command_lifetime/"+group_name);

clients["/hebiros_gazebo_plugin/set_feedback_frequency/"+group_name] =
clients["hebiros_gazebo_plugin/set_feedback_frequency/"+group_name] =
HebirosNode::n_ptr->serviceClient<SetFeedbackFrequencySrv>(
"/hebiros_gazebo_plugin/set_feedback_frequency/"+group_name);
"hebiros_gazebo_plugin/set_feedback_frequency/"+group_name);

clients["/hebiros_gazebo_plugin/acknowledge/"+group_name] =
clients["hebiros_gazebo_plugin/acknowledge/"+group_name] =
HebirosNode::n_ptr->serviceClient<std_srvs::Empty>(
"/hebiros_gazebo_plugin/acknowledge/"+group_name);
"hebiros_gazebo_plugin/acknowledge/"+group_name);
}

bool HebirosClients::addGroup(AddGroupFromNamesSrv::Request &req) {

AddGroupFromNamesSrv srv;
srv.request = req;
return clients["/hebiros_gazebo_plugin/add_group"].call(srv);
return clients["hebiros_gazebo_plugin/add_group"].call(srv);
}

bool HebirosClients::setCommandLifetime(SetCommandLifetimeSrv::Request &req,
std::string group_name) {

SetCommandLifetimeSrv srv;
srv.request = req;
return clients["/hebiros_gazebo_plugin/set_feedback_frequency/"+group_name].call(srv);
return clients["hebiros_gazebo_plugin/set_feedback_frequency/"+group_name].call(srv);
}

bool HebirosClients::setFeedbackFrequency(SetFeedbackFrequencySrv::Request &req,
std::string group_name) {

SetFeedbackFrequencySrv srv;
srv.request = req;
return clients["/hebiros_gazebo_plugin/set_feedback_frequency/"+group_name].call(srv);
return clients["hebiros_gazebo_plugin/set_feedback_frequency/"+group_name].call(srv);
}

bool HebirosClients::acknowledge(std_srvs::Empty::Request &req,
std::string group_name) {

std_srvs::Empty srv;
srv.request = req;
return clients["/hebiros_gazebo_plugin/acknowledge/"+group_name].call(srv);
return clients["hebiros_gazebo_plugin/acknowledge/"+group_name].call(srv);
}
26 changes: 13 additions & 13 deletions hebiros/src/hebiros_parameters.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,27 +4,27 @@


std::map<std::string, bool> HebirosParameters::bool_parameters_default =
{{"/use_sim_time", false}};
{{"use_sim_time", false}};
std::map<std::string, bool> HebirosParameters::bool_parameters;
std::map<std::string, int> HebirosParameters::int_parameters_default =
{{"/hebiros/node_frequency", 200},
{"/hebiros/action_frequency", 200},
{"/hebiros/feedback_frequency", 100},
{"/hebiros/command_lifetime", 100}};
{{"hebiros/node_frequency", 200},
{"hebiros/action_frequency", 200},
{"hebiros/feedback_frequency", 100},
{"hebiros/command_lifetime", 100}};
std::map<std::string, int> HebirosParameters::int_parameters;

void HebirosParameters::setNodeParameters() {

loadInt("/hebiros/node_frequency");
loadInt("/hebiros/action_frequency");
loadInt("/hebiros/feedback_frequency");
loadInt("/hebiros/command_lifetime");
loadInt("hebiros/node_frequency");
loadInt("hebiros/action_frequency");
loadInt("hebiros/feedback_frequency");
loadInt("hebiros/command_lifetime");

ROS_INFO("Parameters:");
ROS_INFO("/hebiros/node_frequency=%d", getInt("/hebiros/node_frequency"));
ROS_INFO("/hebiros/action_frequency=%d", getInt("/hebiros/action_frequency"));
ROS_INFO("/hebiros/feedback_frequency=%d", getInt("/hebiros/feedback_frequency"));
ROS_INFO("/hebiros/command_lifetime=%d", getInt("/hebiros/command_lifetime"));
ROS_INFO("hebiros/node_frequency=%d", getInt("hebiros/node_frequency"));
ROS_INFO("hebiros/action_frequency=%d", getInt("hebiros/action_frequency"));
ROS_INFO("hebiros/feedback_frequency=%d", getInt("hebiros/feedback_frequency"));
ROS_INFO("hebiros/command_lifetime=%d", getInt("hebiros/command_lifetime"));
}

void HebirosParameters::loadBool(std::string name) {
Expand Down
25 changes: 12 additions & 13 deletions hebiros/src/hebiros_publishers.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,30 +10,30 @@ std::map<std::string, ros::Publisher> HebirosPublishers::publishers;

void HebirosPublishers::registerGroupPublishers(std::string group_name) {

publishers["/hebiros/"+group_name+"/feedback"] =
HebirosNode::n_ptr->advertise<FeedbackMsg>("/hebiros/"+group_name+"/feedback", 100);
publishers["hebiros/"+group_name+"/feedback"] =
HebirosNode::n_ptr->advertise<FeedbackMsg>("hebiros/"+group_name+"/feedback", 100);

publishers["/hebiros/"+group_name+"/feedback/joint_state"] =
publishers["hebiros/"+group_name+"/feedback/joint_state"] =
HebirosNode::n_ptr->advertise<sensor_msgs::JointState>(
"/hebiros/"+group_name+"/feedback/joint_state", 100);
"hebiros/"+group_name+"/feedback/joint_state", 100);

publishers["/hebiros/"+group_name+"/feedback/joint_state_urdf"] =
publishers["hebiros/"+group_name+"/feedback/joint_state_urdf"] =
HebirosNode::n_ptr->advertise<sensor_msgs::JointState>(
"/hebiros/"+group_name+"/feedback/joint_state_urdf", 100);
"hebiros/"+group_name+"/feedback/joint_state_urdf", 100);

publishers["/hebiros/"+group_name+"/command/joint_state"] =
publishers["hebiros/"+group_name+"/command/joint_state"] =
HebirosNode::n_ptr->advertise<sensor_msgs::JointState>(
"/hebiros/"+group_name+"/command/joint_state", 100);
"hebiros/"+group_name+"/command/joint_state", 100);
}


void HebirosPublishers::feedback(FeedbackMsg feedback_msg, std::string group_name) {
publishers["/hebiros/"+group_name+"/feedback"].publish(feedback_msg);
publishers["hebiros/"+group_name+"/feedback"].publish(feedback_msg);
}

void HebirosPublishers::feedbackJointState(sensor_msgs::JointState joint_state_msg,
std::string group_name) {
publishers["/hebiros/"+group_name+"/feedback/joint_state"].publish(joint_state_msg);
publishers["hebiros/"+group_name+"/feedback/joint_state"].publish(joint_state_msg);

feedbackJointStateUrdf(joint_state_msg, group_name);
}
Expand All @@ -48,12 +48,11 @@ void HebirosPublishers::feedbackJointStateUrdf(sensor_msgs::JointState joint_sta
joint_state_msg.name[i] = group->joint_full_names[joint_state_msg.name[i]];
}

publishers["/hebiros/"+group_name+"/feedback/joint_state_urdf"].publish(joint_state_msg);
publishers["hebiros/"+group_name+"/feedback/joint_state_urdf"].publish(joint_state_msg);
}
}

void HebirosPublishers::commandJointState(sensor_msgs::JointState joint_state_msg,
std::string group_name) {
publishers["/hebiros/"+group_name+"/command/joint_state"].publish(joint_state_msg);
publishers["hebiros/"+group_name+"/command/joint_state"].publish(joint_state_msg);
}

6 changes: 3 additions & 3 deletions hebiros/src/hebiros_publishers_gazebo.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,15 +7,15 @@ using namespace hebiros;

void HebirosPublishersGazebo::registerGroupPublishers(std::string group_name) {

publishers["/hebiros_gazebo_plugin/command/"+group_name] =
publishers["hebiros_gazebo_plugin/command/"+group_name] =
HebirosNode::n_ptr->advertise<CommandMsg>(
"/hebiros_gazebo_plugin/command/"+group_name, 100);
"hebiros_gazebo_plugin/command/"+group_name, 100);

HebirosPublishers::registerGroupPublishers(group_name);
}

void HebirosPublishersGazebo::command(CommandMsg command_msg, std::string group_name) {

publishers["/hebiros_gazebo_plugin/command/"+group_name].publish(command_msg);
publishers["hebiros_gazebo_plugin/command/"+group_name].publish(command_msg);
}

12 changes: 6 additions & 6 deletions hebiros/src/hebiros_services.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,9 +8,9 @@
std::map<std::string, ros::ServiceServer> HebirosServices::services;

void HebirosServices::registerModelServices(const std::string& model_name) {
services["/hebiros/"+model_name+"/fk"] =
services["hebiros/"+model_name+"/fk"] =
HebirosNode::n_ptr->advertiseService<ModelFkSrv::Request, ModelFkSrv::Response>(
"/hebiros/"+model_name+"/fk",
"hebiros/"+model_name+"/fk",
boost::bind(&HebirosServices::fk, this, _1, _2, model_name));
}

Expand Down Expand Up @@ -124,13 +124,13 @@ bool HebirosServices::size(
if (!registry.hasGroup(group_name))
{
res.size = -1;
ROS_INFO("/hebiros/%s not found; could not get size", group_name.c_str());
ROS_INFO("hebiros/%s not found; could not get size", group_name.c_str());
return false;
}

res.size = registry.getGroup(group_name)->size;

ROS_INFO("/hebiros/%s size=%d", group_name.c_str(), res.size);
ROS_INFO("hebiros/%s size=%d", group_name.c_str(), res.size);

return true;
}
Expand All @@ -139,7 +139,7 @@ bool HebirosServices::setFeedbackFrequency(
SetFeedbackFrequencySrv::Request &req, SetFeedbackFrequencySrv::Response &res,
std::string group_name) {

ROS_INFO("/hebiros/%s feedback_frequency=%d", group_name.c_str(), req.feedback_frequency);
ROS_INFO("hebiros/%s feedback_frequency=%d", group_name.c_str(), req.feedback_frequency);

return true;
}
Expand All @@ -148,7 +148,7 @@ bool HebirosServices::setCommandLifetime(
SetCommandLifetimeSrv::Request &req, SetCommandLifetimeSrv::Response &res,
std::string group_name) {

ROS_INFO("/hebiros/%s command_lifetime=%d", group_name.c_str(), req.command_lifetime);
ROS_INFO("hebiros/%s command_lifetime=%d", group_name.c_str(), req.command_lifetime);

return true;
}
Expand Down
32 changes: 16 additions & 16 deletions hebiros/src/hebiros_services_gazebo.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,42 +6,42 @@

void HebirosServicesGazebo::registerNodeServices() {

services["/hebiros/entry_list"] = HebirosNode::n_ptr->advertiseService(
"/hebiros/entry_list", &HebirosServicesGazebo::entryList, this);
services["hebiros/entry_list"] = HebirosNode::n_ptr->advertiseService(
"hebiros/entry_list", &HebirosServicesGazebo::entryList, this);

services["/hebiros/add_group_from_names"] = HebirosNode::n_ptr->advertiseService(
"/hebiros/add_group_from_names", &HebirosServicesGazebo::addGroupFromNames, this);
services["hebiros/add_group_from_names"] = HebirosNode::n_ptr->advertiseService(
"hebiros/add_group_from_names", &HebirosServicesGazebo::addGroupFromNames, this);

services["/hebiros/add_group_from_urdf"] = HebirosNode::n_ptr->advertiseService(
"/hebiros/add_group_from_urdf", &HebirosServicesGazebo::addGroupFromURDF, this);
services["hebiros/add_group_from_urdf"] = HebirosNode::n_ptr->advertiseService(
"hebiros/add_group_from_urdf", &HebirosServicesGazebo::addGroupFromURDF, this);

services["/hebiros/add_model_from_urdf"] = HebirosNode::n_ptr->advertiseService(
"/hebiros/add_model_from_urdf", &HebirosServicesGazebo::addModelFromURDF, this);
services["hebiros/add_model_from_urdf"] = HebirosNode::n_ptr->advertiseService(
"hebiros/add_model_from_urdf", &HebirosServicesGazebo::addModelFromURDF, this);
}

void HebirosServicesGazebo::registerGroupServices(std::string group_name) {

services["/hebiros/"+group_name+"/size"] =
services["hebiros/"+group_name+"/size"] =
HebirosNode::n_ptr->advertiseService<SizeSrv::Request, SizeSrv::Response>(
"/hebiros/"+group_name+"/size",
"hebiros/"+group_name+"/size",
boost::bind(&HebirosServicesGazebo::size, this, _1, _2, group_name));

services["/hebiros/"+group_name+"/set_feedback_frequency"] =
services["hebiros/"+group_name+"/set_feedback_frequency"] =
HebirosNode::n_ptr->advertiseService<SetFeedbackFrequencySrv::Request,
SetFeedbackFrequencySrv::Response>(
"/hebiros/"+group_name+"/set_feedback_frequency",
"hebiros/"+group_name+"/set_feedback_frequency",
boost::bind(&HebirosServicesGazebo::setFeedbackFrequency, this, _1, _2, group_name));

services["/hebiros/"+group_name+"/set_command_lifetime"] =
services["hebiros/"+group_name+"/set_command_lifetime"] =
HebirosNode::n_ptr->advertiseService<SetCommandLifetimeSrv::Request,
SetCommandLifetimeSrv::Response>(
"/hebiros/"+group_name+"/set_command_lifetime",
"hebiros/"+group_name+"/set_command_lifetime",
boost::bind(&HebirosServicesGazebo::setCommandLifetime, this, _1, _2, group_name));

services["/hebiros/"+group_name+"/send_command_with_acknowledgement"] =
services["hebiros/"+group_name+"/send_command_with_acknowledgement"] =
HebirosNode::n_ptr->advertiseService<SendCommandWithAcknowledgementSrv::Request,
SendCommandWithAcknowledgementSrv::Response>(
"/hebiros/"+group_name+"/send_command_with_acknowledgement",
"hebiros/"+group_name+"/send_command_with_acknowledgement",
boost::bind(&HebirosServicesGazebo::sendCommandWithAcknowledgement, this, _1, _2, group_name));
}

Expand Down
32 changes: 16 additions & 16 deletions hebiros/src/hebiros_services_physical.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,42 +6,42 @@

void HebirosServicesPhysical::registerNodeServices() {

services["/hebiros/entry_list"] = HebirosNode::n_ptr->advertiseService(
"/hebiros/entry_list", &HebirosServicesPhysical::entryList, this);
services["hebiros/entry_list"] = HebirosNode::n_ptr->advertiseService(
"hebiros/entry_list", &HebirosServicesPhysical::entryList, this);

services["/hebiros/add_group_from_names"] = HebirosNode::n_ptr->advertiseService(
"/hebiros/add_group_from_names", &HebirosServicesPhysical::addGroupFromNames, this);
services["hebiros/add_group_from_names"] = HebirosNode::n_ptr->advertiseService(
"hebiros/add_group_from_names", &HebirosServicesPhysical::addGroupFromNames, this);

services["/hebiros/add_group_from_urdf"] = HebirosNode::n_ptr->advertiseService(
"/hebiros/add_group_from_urdf", &HebirosServicesPhysical::addGroupFromURDF, this);
services["hebiros/add_group_from_urdf"] = HebirosNode::n_ptr->advertiseService(
"hebiros/add_group_from_urdf", &HebirosServicesPhysical::addGroupFromURDF, this);

services["/hebiros/add_model_from_urdf"] = HebirosNode::n_ptr->advertiseService(
"/hebiros/add_model_from_urdf", &HebirosServicesPhysical::addModelFromURDF, this);
services["hebiros/add_model_from_urdf"] = HebirosNode::n_ptr->advertiseService(
"hebiros/add_model_from_urdf", &HebirosServicesPhysical::addModelFromURDF, this);
}

void HebirosServicesPhysical::registerGroupServices(std::string group_name) {

services["/hebiros/"+group_name+"/size"] =
services["hebiros/"+group_name+"/size"] =
HebirosNode::n_ptr->advertiseService<SizeSrv::Request, SizeSrv::Response>(
"/hebiros/"+group_name+"/size",
"hebiros/"+group_name+"/size",
boost::bind(&HebirosServicesPhysical::size, this, _1, _2, group_name));

services["/hebiros/"+group_name+"/set_feedback_frequency"] =
services["hebiros/"+group_name+"/set_feedback_frequency"] =
HebirosNode::n_ptr->advertiseService<SetFeedbackFrequencySrv::Request,
SetFeedbackFrequencySrv::Response>(
"/hebiros/"+group_name+"/set_feedback_frequency",
"hebiros/"+group_name+"/set_feedback_frequency",
boost::bind(&HebirosServicesPhysical::setFeedbackFrequency, this, _1, _2, group_name));

services["/hebiros/"+group_name+"/set_command_lifetime"] =
services["hebiros/"+group_name+"/set_command_lifetime"] =
HebirosNode::n_ptr->advertiseService<SetCommandLifetimeSrv::Request,
SetCommandLifetimeSrv::Response>(
"/hebiros/"+group_name+"/set_command_lifetime",
"hebiros/"+group_name+"/set_command_lifetime",
boost::bind(&HebirosServicesPhysical::setCommandLifetime, this, _1, _2, group_name));

services["/hebiros/"+group_name+"/send_command_with_acknowledgement"] =
services["hebiros/"+group_name+"/send_command_with_acknowledgement"] =
HebirosNode::n_ptr->advertiseService<SendCommandWithAcknowledgementSrv::Request,
SendCommandWithAcknowledgementSrv::Response>(
"/hebiros/"+group_name+"/send_command_with_acknowledgement",
"hebiros/"+group_name+"/send_command_with_acknowledgement",
boost::bind(&HebirosServicesPhysical::sendCommandWithAcknowledgement, this, _1, _2, group_name));
}

Expand Down
Loading