Skip to content

Commit

Permalink
WIP #229 updated generated code to use ros instead of rosmod; updated…
Browse files Browse the repository at this point in the history
… RunExperiment to call "rosrun rosmod_actor rosmod_actor" instead of assuming rosmod_actor is in the PATH.
  • Loading branch information
finger563 committed Apr 6, 2018
1 parent f35d167 commit c5ccbf2
Show file tree
Hide file tree
Showing 5 changed files with 48 additions and 94 deletions.
6 changes: 3 additions & 3 deletions src/plugins/RunExperiment/RunExperiment.js
Original file line number Diff line number Diff line change
Expand Up @@ -684,7 +684,7 @@ define([

var redirect_command = ' > ' + self.configPrefix + node.name + '.stdout.log' +
' 2> ' + self.configPrefix + node.name + '.stderr.log';
rosmod_actor_script.push('nohup rosmod_actor --config ' +
rosmod_actor_script.push('nohup rosrun rosmod_actor rosmod_actor --config ' +
nodeConfigName + redirect_command +' &');

});
Expand Down Expand Up @@ -843,7 +843,7 @@ define([
'\x1b[107;90m# on the server machine:\n'+
'# you will need to scp rosmod_actor from the target\n' +
'\x1b[107;94m$ \x1b[40;92m'+
`<span style="user-select:text;">scp -i ${host.user.Key} ${host.user.name}@${host.intf.IP}:/opt/rosmod/bin/rosmod_actor .\n</span>` +
`<span style="user-select:text;">scp -i ${host.user.Key} ${host.user.name}@${host.intf.IP}:/opt/rosmod/lib/rosmod_actor/rosmod_actor .\n</span>` +
'\x1b[107;94m$ \x1b[40;92m'+
'<span style="user-select:text;">gdb-multiarch ./rosmod_actor\n</span>' +
'\x1b[107;90m# then within gdb:\n' +
Expand Down Expand Up @@ -905,7 +905,7 @@ define([
host_commands.push('nohup ' +
valgrind_command +
gdbserver_command +
' rosmod_actor --config ' +
' rosrun rosmod_actor rosmod_actor --config ' +
nodeConfigName +
args +
redirect_command +
Expand Down
2 changes: 1 addition & 1 deletion src/plugins/SoftwareGenerator/Templates/CMakeLists.txt.ejs
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@ if (pkgInfo.Message_list || pkgInfo.Service_list || pkgInfo.Action_list) {
<%
}
-%>
rosmod
roscpp
rosmod_actor
# external packages that we depend on
<%- pkgInfo.PackageDependencies.join(' ') %>
Expand Down
6 changes: 3 additions & 3 deletions src/plugins/SoftwareGenerator/Templates/Templates.js

Large diffs are not rendered by default.

108 changes: 31 additions & 77 deletions src/plugins/SoftwareGenerator/Templates/component.cpp.ejs
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@ if (compInfo['State Machine_list']) {
<%- compInfo.Definitions %>

// Component Initialization
void <%- compInfo.name %>::init_timer_operation(const rosmod::TimerEvent& event)
void <%- compInfo.name %>::init_timer_operation(const ros::TimerEvent& event)
{
// User Initialization Code
try {
Expand Down Expand Up @@ -72,7 +72,7 @@ if (compInfo['State Machine_list']) {
compInfo['State Machine_list'].map(function(hfsm) {
-%>
// <%- hfsm.name %> Timer function
void <%- compInfo.name %>::<%- hfsm.sanitizedName %>_HFSM_timer_operation(const rosmod::TimerEvent& event)
void <%- compInfo.name %>::<%- hfsm.sanitizedName %>_HFSM_timer_operation(const ros::TimerEvent& event)
{
try {
StateMachine::Event* e = nullptr;
Expand Down Expand Up @@ -113,7 +113,7 @@ if (compInfo.Timer_list) {
compInfo.Timer_list.map(function(tmr) {
-%>
// Timer Operation - <%- tmr.name %>
void <%- compInfo.name %>::<%- tmr.name %>_operation(const rosmod::TimerEvent& event)
void <%- compInfo.name %>::<%- tmr.name %>_operation(const ros::TimerEvent& event)
{
try {
//::::<%- tmr.path %>::::Operation::::
Expand Down Expand Up @@ -370,33 +370,33 @@ if (compInfo['Action Server_list']) {
// Startup - Setup Component Ports & Timers
void <%- compInfo.name %>::startUp()
{
rosmod::NodeHandle nh;
ros::NodeHandle nh;
std::string advertiseName;
ros::Duration deadline;
bool isOneShot;
rosmod::TimerOptions timer_options;
rosmod::ROSMOD_Callback_Options callback_options;
ros::TimerOptions timer_options;
// extra data we add to the header for service connections
std::map<std::string, std::string> service_header;
service_header["component instance"] = config["Name"].asString();
if ( config["Logging"]["Component Logger"]["Enabled"].asBool() ) {
logger->enable_logging();
logger->create_file(workingDir + "/" + config["Logging"]["Component Logger"]["FileName"].asString());
logger->set_is_periodic(config["Logging"]["Component Logger"]["Enabled"].asBool());
logger->set_max_log_unit(config["Logging"]["Component Logger"]["Unit"].asInt());
ROS_INFO_STREAM("Saving trace log to " << workingDir << "/" << config["Logging"]["Component Logger"]["FileName"].asString());
}
if ( config["Logging"]["ROSMOD Logger"]["Enabled"].asBool() ) {
comp_queue.LOGGER->enable_logging();
comp_queue.LOGGER->create_file( workingDir + "/" + config["Logging"]["ROSMOD Logger"]["FileName"].asString());
comp_queue.LOGGER->set_is_periodic(config["Logging"]["ROSMOD Logger"]["Enabled"].asBool());
comp_queue.LOGGER->set_max_log_unit(config["Logging"]["ROSMOD Logger"]["Unit"].asInt());
ROS_INFO_STREAM("Saving trace log to " << workingDir + "/" + config["Logging"]["ROSMOD Logger"]["FileName"].asString());
comp_queue.ROSMOD_LOGGER->enable_logging();
comp_queue.ROSMOD_LOGGER->create_file( workingDir + "/" + config["Logging"]["ROSMOD Logger"]["FileName"].asString());
comp_queue.ROSMOD_LOGGER->set_max_log_unit(config["Logging"]["ROSMOD Logger"]["Unit"].asInt());
}
this->comp_queue.scheduling_scheme = config["SchedulingScheme"].asString();
// Action Servers
<%
Expand All @@ -409,15 +409,8 @@ if (compInfo['Action Server_list']) {
this-><%- srv.name %>.initialize(
advertiseName,
boost::bind(&<%- compInfo.name %>::<%- srv.name %>_execute_cb, this, _1),
false // don't auto-start the action server
true // don't auto-start the action server
);
this-><%- srv.name %>.registerGoalCallback(
boost::bind(&<%- compInfo.name %>::<%- srv.name %>_goal_cb, this)
);
this-><%- srv.name %>.registerPreemptCallback(
boost::bind(&<%- compInfo.name %>::<%- srv.name %>_preempt_cb, this)
);
this-><%- srv.name %>.start();
<%
});
}
Expand All @@ -430,11 +423,6 @@ if (compInfo['Action Client_list']) {
// Action Client - <%- clt.name %>
// TODO: Add support for callback options (Priority / Deadline) to Action Client Callbacks
advertiseName = "<%- clt.Action.AdvertisedName %>";
this-><%- clt.name %>.initialize(
((ros::NodeHandle&)nh),
advertiseName,
((ros::CallbackQueue*)&this->comp_queue)
);
this-><%- clt.name %>.registerFeedbackCallback(
boost::bind(&<%- compInfo.name %>::<%- clt.name %>_feedback_cb, this, _1)
);
Expand All @@ -444,7 +432,12 @@ if (compInfo['Action Client_list']) {
this-><%- clt.name %>.registerActiveCallback(
boost::bind(&<%- compInfo.name %>::<%- clt.name %>_active_cb, this)
);
this-><%- clt.name %>.waitForServer();
this-><%- clt.name %>.initialize(
nh,
advertiseName,
&this->comp_queue
);
//this-><%- clt.name %>.waitForServer();
logger->log("DEBUG", "<%- clt.name %> connected!");
<%
});
Expand All @@ -456,21 +449,13 @@ if (compInfo.Server_list) {
compInfo.Server_list.map(function(srv) {
-%>
// Server - <%- srv.name %>
callback_options.alias = "<%- srv.name %>_operation";
callback_options.priority = config["Servers"]["<%- srv.name %>"]["Priority"].asInt();
deadline = ros::Duration(config["Servers"]["<%- srv.name %>"]["Deadline"].asFloat());
callback_options.deadline.sec = deadline.sec;
callback_options.deadline.nsec = deadline.nsec;
advertiseName = "<%- srv.Service.AdvertisedName %>";
rosmod::AdvertiseServiceOptions <%- srv.name %>_server_options;
<%- srv.name %>_server_options = rosmod::AdvertiseServiceOptions::create<<%- srv.Service.Package %>::<%- srv.Service.TypeName %>>
ros::AdvertiseServiceOptions <%- srv.name %>_server_options;
<%- srv.name %>_server_options = ros::AdvertiseServiceOptions::create<<%- srv.Service.Package %>::<%- srv.Service.TypeName %>>
(advertiseName.c_str(),
boost::bind(&<%- compInfo.name %>::<%- srv.name %>_operation, this, _1, _2),
rosmod::VoidPtr(),
&this->comp_queue,
callback_options);
ros::VoidPtr(),
&this->comp_queue);
this-><%- srv.name %> = nh.advertiseService(<%- srv.name %>_server_options);
<%
});
Expand Down Expand Up @@ -518,43 +503,26 @@ if (compInfo.Subscriber_list) {
compInfo.Subscriber_list.map(function(sub) {
-%>
// Subscriber - <%- sub.name %>
callback_options.alias = "<%- sub.name %>_operation";
callback_options.priority = config["Subscribers"]["<%- sub.name %>"]["Priority"].asInt();
deadline = ros::Duration(config["Subscribers"]["<%- sub.name %>"]["Deadline"].asFloat());
callback_options.deadline.sec = deadline.sec;
callback_options.deadline.nsec = deadline.nsec;
callback_options.priority = <%- sub.Priority %>;
callback_options.deadline.sec = <%- Math.floor(sub.Deadline) %>;
callback_options.deadline.nsec = <%- Math.floor((sub.Deadline % 1) * 1000000000) %>;
advertiseName = "<%- sub.Message.AdvertisedName %>";
rosmod::SubscribeOptions <%- sub.name %>_options;
<%- sub.name %>_options = rosmod::SubscribeOptions::create<<%- sub.Message.Package %>::<%- sub.Message.TypeName %>>
ros::SubscribeOptions <%- sub.name %>_options;
<%- sub.name %>_options = ros::SubscribeOptions::create<<%- sub.Message.Package %>::<%- sub.Message.TypeName %>>
(advertiseName.c_str(),
1000,
boost::bind(&<%- compInfo.name %>::<%- sub.name %>_operation, this, _1),
rosmod::VoidPtr(),
&this->comp_queue,
callback_options);
ros::VoidPtr(),
&this->comp_queue);
this-><%- sub.name %> = nh.subscribe(<%- sub.name %>_options);
<%
});
}
-%>
// Init Timer
callback_options.alias = "init_timer_operation";
callback_options.priority = 99;
callback_options.deadline.sec = 1;
callback_options.deadline.nsec = 0;
timer_options =
rosmod::TimerOptions
ros::TimerOptions
(ros::Duration(-1),
boost::bind(&<%- compInfo.name %>::init_timer_operation, this, _1),
&this->comp_queue,
callback_options,
true,
false);
this->init_timer = nh.createTimer(timer_options);
Expand All @@ -566,19 +534,12 @@ if (compInfo.Timer_list) {
compInfo.Timer_list.map(function(tmr) {
-%>
// Component Timer - <%- tmr.name %>
callback_options.alias = "<%- tmr.name %>_operation";
callback_options.priority = config["Timers"]["<%- tmr.name %>"]["Priority"].asInt();
deadline = ros::Duration(config["Timers"]["<%- tmr.name %>"]["Deadline"].asFloat());
callback_options.deadline.sec = deadline.sec;
callback_options.deadline.nsec = deadline.nsec;
isOneShot = (config["Timers"]["<%- tmr.name %>"]["Period"].asFloat() == 0) ? true : false;
timer_options =
rosmod::TimerOptions
ros::TimerOptions
(ros::Duration(config["Timers"]["<%- tmr.name %>"]["Period"].asFloat()),
boost::bind(&<%- compInfo.name %>::<%- tmr.name %>_operation, this, _1),
&this->comp_queue,
callback_options,
isOneShot,
false);
this-><%- tmr.name %> = nh.createTimer(timer_options);
Expand All @@ -592,21 +553,14 @@ if (compInfo['State Machine_list']) {
compInfo['State Machine_list'].map(function(hfsm) {
-%>
// HFSM Timer
callback_options.alias = "<%- hfsm.sanitizedName %>_HFSM_timer_operation";
// NEED TO FIGURE OUT HOW TO SPECIFY PRIORITY FOR THE HFSM TIMER IN THE MODEL
//callback_options.priority = config["Timers"]["<%- hfsm.sanitizedName %>"]["Priority"].asInt();
ros::Duration hfsmPeriod = ros::Duration( 0 );
deadline = hfsmPeriod;
callback_options.deadline.sec = deadline.sec;
callback_options.deadline.nsec = deadline.nsec;
isOneShot = false;
timer_options =
rosmod::TimerOptions
ros::TimerOptions
(hfsmPeriod,
boost::bind(&<%- compInfo.name %>::<%- hfsm.sanitizedName %>_HFSM_timer_operation, this, _1),
&this->comp_queue,
callback_options,
isOneShot,
false);
<%- hfsm.sanitizedName %>_HFSM_timer = nh.createTimer(timer_options);
Expand Down
20 changes: 10 additions & 10 deletions src/plugins/SoftwareGenerator/Templates/component.hpp.ejs
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,7 @@ if (compInfo.Types.length) {
}
-%>

#include "rosmod/rosmod_ros.h"
#include "ros/ros.h"

/**
* Forward declarations
Expand Down Expand Up @@ -74,7 +74,7 @@ if (compInfo['State Machine_list']) {
* @param[in] event a oneshot timer event
* @see startUp()
*/
void init_timer_operation(const rosmod::TimerEvent& event);
void init_timer_operation(const ros::TimerEvent& event);

<%
if (compInfo['State Machine_list']) {
Expand All @@ -90,7 +90,7 @@ if (compInfo['State Machine_list']) {
* @param[in] event a timer event
* @see startUp()
*/
void <%- hfsm.sanitizedName %>_HFSM_timer_operation(const rosmod::TimerEvent& event);
void <%- hfsm.sanitizedName %>_HFSM_timer_operation(const ros::TimerEvent& event);
<%
});
}
Expand All @@ -109,7 +109,7 @@ if (compInfo.Timer_list) {
*
* <%- tmr['Detailed Description'] %>
*/
void <%- tmr.name %>_operation(const rosmod::TimerEvent& event);
void <%- tmr.name %>_operation(const ros::TimerEvent& event);
<%
});
}
Expand Down Expand Up @@ -238,7 +238,7 @@ private:
if (compInfo['State Machine_list']) {
compInfo['State Machine_list'].map(function(hfsm) {
-%>
rosmod::Timer <%- hfsm.sanitizedName %>_HFSM_timer; /*!< <%- hfsm.name %> HFSM Timer */
ros::Timer <%- hfsm.sanitizedName %>_HFSM_timer; /*!< <%- hfsm.name %> HFSM Timer */
<%
});
}
Expand All @@ -248,7 +248,7 @@ if (compInfo['State Machine_list']) {
if (compInfo.Timer_list) {
compInfo.Timer_list.map(function(tmr) {
-%>
rosmod::Timer <%- tmr.name %>; /*!< <%- tmr.name %> Component Timer */
ros::Timer <%- tmr.name %>; /*!< <%- tmr.name %> Component Timer */
<%
});
}
Expand All @@ -275,7 +275,7 @@ if (compInfo['Action Client_list']) {
if (compInfo.Server_list) {
compInfo.Server_list.map(function(srv) {
-%>
rosmod::ServiceServer <%- srv.name %>; /*!< <%- srv.name %> Component Server */
ros::ServiceServer <%- srv.name %>; /*!< <%- srv.name %> Component Server */
<%
});
}
Expand All @@ -284,7 +284,7 @@ if (compInfo.Server_list) {
if (compInfo.Client_list) {
compInfo.Client_list.map(function(clt) {
-%>
rosmod::ServiceClient <%- clt.name %>; /*!< <%- clt.name %> Component Client */
ros::ServiceClient <%- clt.name %>; /*!< <%- clt.name %> Component Client */
<%
});
}
Expand All @@ -293,7 +293,7 @@ if (compInfo.Client_list) {
if (compInfo.Publisher_list) {
compInfo.Publisher_list.map(function(pub) {
-%>
rosmod::Publisher <%- pub.name %>; /*!< <%- pub.name %> Component Publisher */
ros::Publisher <%- pub.name %>; /*!< <%- pub.name %> Component Publisher */
<%
});
}
Expand All @@ -302,7 +302,7 @@ if (compInfo.Publisher_list) {
if (compInfo.Subscriber_list) {
compInfo.Subscriber_list.map(function(sub) {
-%>
rosmod::Subscriber <%- sub.name %>; /*!< <%- sub.name %> Component Subscriber */
ros::Subscriber <%- sub.name %>; /*!< <%- sub.name %> Component Subscriber */
<%
});
}
Expand Down

0 comments on commit c5ccbf2

Please sign in to comment.