bool initInterfaces()
{
ROS_FATAL_NAMED(APPLICATION_NAME, "CIAOOOOOOOOOOOOOOOOO\n");
int j;
for(std::map<std::string,boost::shared_ptr<urdf::Joint> >::iterator joint = robot_model.joints_.begin(); joint != robot_model.joints_.end(); joint++)
{
printf("%s\n", joint->first.c_str());
j = jointMap[joint->first];
// connect and register the joint state interface
hardware_interface::JointStateHandle state_handle_tmp(joint->first.c_str(), &measPos[j], &measVel[j], &measEff[j]);
jnt_state_interface.registerHandle(state_handle_tmp);
// connect and register the joint position interface
hardware_interface::JointHandle pos_handle_tmp(jnt_state_interface.getHandle(joint->first.c_str()), &outCmd[j]);
jnt_pos_interface.registerHandle(pos_handle_tmp);
// connect and register the joint effort interface
hardware_interface::JointHandle eff_handle_tmp(jnt_state_interface.getHandle(joint->first.c_str()), &outCmd[j]);
eff_pos_interface.registerHandle(eff_handle_tmp);
}
registerInterface(&jnt_state_interface);
registerInterface(&jnt_pos_interface);
registerInterface(&eff_pos_interface);
}
GripperRosControl(const std::vector<std::string>& actr_names, const std::vector<std::string>& jnt_names,
const std::vector<std::string>& controller_names, const std::vector<double>& reducers)
: actr_names_(actr_names)
, jnt_names_(jnt_names)
, controller_names_(controller_names)
{
for (int i = 0; i < jnt_names_.size(); i++)
{
std::string actr = actr_names_[i];
std::string jnt = jnt_names_[i];
// Get transmission
boost::shared_ptr<transmission_interface::SimpleTransmission> t_ptr(
new transmission_interface::SimpleTransmission(reducers[i]));
trans_.push_back(t_ptr);
// Initialize and wrap raw current data
actr_curr_data_[actr].position.push_back(&actr_curr_pos_[actr]);
actr_curr_data_[actr].velocity.push_back(&actr_curr_vel_[actr]);
actr_curr_data_[actr].effort.push_back(&actr_curr_eff_[actr]);
jnt_curr_data_[jnt].position.push_back(&jnt_curr_pos_[jnt]);
jnt_curr_data_[jnt].velocity.push_back(&jnt_curr_vel_[jnt]);
jnt_curr_data_[jnt].effort.push_back(&jnt_curr_eff_[jnt]);
// Initialize and wrap raw command data
actr_cmd_data_[actr].position.push_back(&actr_cmd_pos_[actr]);
jnt_cmd_data_[jnt].position.push_back(&jnt_cmd_pos_[jnt]);
// Register transmissions to each interface
actr_to_jnt_state_.registerHandle(transmission_interface::ActuatorToJointStateHandle(
actr + "_trans", trans_[i].get(), actr_curr_data_[actr], jnt_curr_data_[jnt]));
jnt_to_actr_pos_.registerHandle(transmission_interface::JointToActuatorPositionHandle(
jnt + "_trans", trans_[i].get(), actr_cmd_data_[actr], jnt_cmd_data_[jnt]));
// Connect and register the joint state handle
hardware_interface::JointStateHandle state_handle(jnt, &jnt_curr_pos_[jnt], &jnt_curr_vel_[jnt],
&jnt_curr_eff_[jnt]);
jnt_state_interface_.registerHandle(state_handle);
// Connect and register the joint position handle
hardware_interface::JointHandle pos_handle(jnt_state_interface_.getHandle(jnt), &jnt_cmd_pos_[jnt]);
pos_jnt_interface_.registerHandle(pos_handle);
// ROS publishers and subscribers
actr_cmd_pub_[actr] = nh_.advertise<std_msgs::Float64>("dxl/" + controller_names_[i] + "/command", 5);
actr_state_sub_[actr] =
nh_.subscribe("dxl/" + controller_names_[i] + "/state", 1, &GripperRosControl::actrStateCallback, this);
}
// Register interfaces
registerInterface(&jnt_state_interface_);
registerInterface(&pos_jnt_interface_);
// Start spinning
nh_.setCallbackQueue(&subscriber_queue_);
subscriber_spinner_.reset(new ros::AsyncSpinner(1, &subscriber_queue_));
subscriber_spinner_->start();
}
MitsubishiArmInterface::MitsubishiArmInterface(std::string & port)
{
//joint_state_pub=n_priv.advertise<sensor_msgs::JointState>( "joint_states", 1);
pos.resize(joint_number);
vel.resize(joint_number);
eff.resize(joint_number);
cmd.resize(joint_number);
// connect and register the joint state interface
hardware_interface::JointStateHandle state_handle_j1("j1", &pos[0], &vel[0], &eff[0]);
jnt_state_interface.registerHandle(state_handle_j1);
hardware_interface::JointStateHandle state_handle_j2("j2", &pos[1], &vel[1], &eff[1]);
jnt_state_interface.registerHandle(state_handle_j2);
hardware_interface::JointStateHandle state_handle_j3("j3", &pos[2], &vel[2], &eff[2]);
jnt_state_interface.registerHandle(state_handle_j3);
hardware_interface::JointStateHandle state_handle_j4("j4", &pos[3], &vel[3], &eff[3]);
jnt_state_interface.registerHandle(state_handle_j4);
hardware_interface::JointStateHandle state_handle_j5("j5", &pos[4], &vel[4], &eff[4]);
jnt_state_interface.registerHandle(state_handle_j5);
hardware_interface::JointStateHandle state_handle_j6("j6", &pos[5], &vel[5], &eff[5]);
jnt_state_interface.registerHandle(state_handle_j6);
registerInterface(&jnt_state_interface);
// connect and register the joint position interface
hardware_interface::JointHandle pos_handle_j1(jnt_state_interface.getHandle("j1"), &cmd[0]);
jnt_pos_interface.registerHandle(pos_handle_j1);
hardware_interface::JointHandle pos_handle_j2(jnt_state_interface.getHandle("j2"), &cmd[1]);
jnt_pos_interface.registerHandle(pos_handle_j2);
hardware_interface::JointHandle pos_handle_j3(jnt_state_interface.getHandle("j3"), &cmd[2]);
jnt_pos_interface.registerHandle(pos_handle_j3);
hardware_interface::JointHandle pos_handle_j4(jnt_state_interface.getHandle("j4"), &cmd[3]);
jnt_pos_interface.registerHandle(pos_handle_j4);
hardware_interface::JointHandle pos_handle_j5(jnt_state_interface.getHandle("j5"), &cmd[4]);
jnt_pos_interface.registerHandle(pos_handle_j5);
hardware_interface::JointHandle pos_handle_j6(jnt_state_interface.getHandle("j6"), &cmd[5]);
jnt_pos_interface.registerHandle(pos_handle_j6);
registerInterface(&jnt_pos_interface);
cmd=pos;
cmd_previous=cmd;
}
void UrHardwareInterface::init() {
ROS_INFO_STREAM_NAMED("ur_hardware_interface",
"Reading rosparams from namespace: " << nh_.getNamespace());
// Get joint names
nh_.getParam("hardware_interface/joints", joint_names_);
if (joint_names_.size() == 0) {
ROS_FATAL_STREAM_NAMED("ur_hardware_interface",
"No joints found on parameter server for controller, did you load the proper yaml file?" << " Namespace: " << nh_.getNamespace());
exit(-1);
}
num_joints_ = joint_names_.size();
// Resize vectors
joint_position_.resize(num_joints_);
joint_velocity_.resize(num_joints_);
joint_effort_.resize(num_joints_);
joint_position_command_.resize(num_joints_);
joint_velocity_command_.resize(num_joints_);
// Initialize controller
for (std::size_t i = 0; i < num_joints_; ++i) {
ROS_DEBUG_STREAM_NAMED("ur_hardware_interface",
"Loading joint name: " << joint_names_[i]);
// Create joint state interface
joint_state_interface_.registerHandle(
hardware_interface::JointStateHandle(joint_names_[i],
&joint_position_[i], &joint_velocity_[i],
&joint_effort_[i]));
// Create position joint interface
position_joint_interface_.registerHandle(
hardware_interface::JointHandle(
joint_state_interface_.getHandle(joint_names_[i]),
&joint_position_command_[i]));
// Create velocity joint interface
velocity_joint_interface_.registerHandle(
hardware_interface::JointHandle(
joint_state_interface_.getHandle(joint_names_[i]),
&joint_velocity_command_[i]));
}
// Create force torque interface
force_torque_interface_.registerHandle(
hardware_interface::ForceTorqueSensorHandle("wrench", "",
robot_force_, robot_torque_));
registerInterface(&joint_state_interface_); // From RobotHW base class.
registerInterface(&position_joint_interface_); // From RobotHW base class.
registerInterface(&velocity_joint_interface_); // From RobotHW base class.
registerInterface(&force_torque_interface_); // From RobotHW base class.
velocity_interface_running_ = false;
position_interface_running_ = false;
}
MyRobot()
{
// connect and register the joint state interface
hardware_interface::JointStateHandle state_handle_lwa1("lwa/joint_1", &pos[0], &vel[0], &eff[0]);
jnt_state_interface.registerHandle(state_handle_lwa1);
hardware_interface::JointStateHandle state_handle_lwa2("lwa/joint_2", &pos[1], &vel[1], &eff[1]);
jnt_state_interface.registerHandle(state_handle_lwa2);
hardware_interface::JointStateHandle state_handle_lwa3("lwa/joint_3", &pos[2], &vel[2], &eff[2]);
jnt_state_interface.registerHandle(state_handle_lwa3);
hardware_interface::JointStateHandle state_handle_lwa4("lwa/joint_4", &pos[3], &vel[3], &eff[3]);
jnt_state_interface.registerHandle(state_handle_lwa4);
hardware_interface::JointStateHandle state_handle_lwa5("lwa/joint_5", &pos[4], &vel[4], &eff[4]);
jnt_state_interface.registerHandle(state_handle_lwa5);
hardware_interface::JointStateHandle state_handle_lwa6("lwa/joint_6", &pos[5], &vel[5], &eff[5]);
jnt_state_interface.registerHandle(state_handle_lwa6);
hardware_interface::JointStateHandle state_handle_lwa7("lwa/joint_7", &pos[6], &vel[6], &eff[6]);
jnt_state_interface.registerHandle(state_handle_lwa7);
registerInterface(&jnt_state_interface);
// connect and register the joint position interface
hardware_interface::JointHandle pos_handle_lwa1(jnt_state_interface.getHandle("lwa/joint_1"), &cmd[0]);
jnt_pos_interface.registerHandle(pos_handle_lwa1);
hardware_interface::JointHandle pos_handle_lwa2(jnt_state_interface.getHandle("lwa/joint_2"), &cmd[1]);
jnt_pos_interface.registerHandle(pos_handle_lwa2);
hardware_interface::JointHandle pos_handle_lwa3(jnt_state_interface.getHandle("lwa/joint_3"), &cmd[2]);
jnt_pos_interface.registerHandle(pos_handle_lwa3);
hardware_interface::JointHandle pos_handle_lwa4(jnt_state_interface.getHandle("lwa/joint_4"), &cmd[3]);
jnt_pos_interface.registerHandle(pos_handle_lwa4);
hardware_interface::JointHandle pos_handle_lwa5(jnt_state_interface.getHandle("lwa/joint_5"), &cmd[4]);
jnt_pos_interface.registerHandle(pos_handle_lwa5);
hardware_interface::JointHandle pos_handle_lwa6(jnt_state_interface.getHandle("lwa/joint_6"), &cmd[5]);
jnt_pos_interface.registerHandle(pos_handle_lwa6);
hardware_interface::JointHandle pos_handle_lwa7(jnt_state_interface.getHandle("lwa/joint_7"), &cmd[6]);
jnt_pos_interface.registerHandle(pos_handle_lwa7);
registerInterface(&jnt_pos_interface);
}
Hsrdp()
{
// connect and register the joint state interface
hardware_interface::JointStateHandle sh_shoulder_joint("shoulder_joint", &pos[0], &vel[0], &eff[0]);
jnt_state_interface.registerHandle(sh_shoulder_joint);
hardware_interface::JointStateHandle sh_wrist_gripper_connection_pitch("wrist_gripper_connection_pitch", &pos[1], &vel[1], &eff[1]);
jnt_state_interface.registerHandle(sh_wrist_gripper_connection_pitch);
hardware_interface::JointStateHandle sh_wrist_gripper_connection_roll("wrist_gripper_connection_roll", &pos[2], &vel[2], &eff[2]);
jnt_state_interface.registerHandle(sh_wrist_gripper_connection_roll);
hardware_interface::JointStateHandle sh_shoulder_updown("shoulder_updown", &pos[3], &vel[3], &eff[3]);
jnt_state_interface.registerHandle(sh_shoulder_updown);
hardware_interface::JointStateHandle sh_elbow("elbow", &pos[4], &vel[4], &eff[4]);
jnt_state_interface.registerHandle(sh_elbow);
hardware_interface::JointStateHandle sh_wrist("wrist", &pos[5], &vel[5], &eff[5]);
jnt_state_interface.registerHandle(sh_wrist);
hardware_interface::JointStateHandle sh_gripper("gripper", &pos[6], &vel[6], &eff[6]);
jnt_state_interface.registerHandle(sh_gripper);
registerInterface(&jnt_state_interface);
hardware_interface::JointHandle ph_shoulder_joint(jnt_state_interface.getHandle("shoulder_joint"), &cmd[0]);
jnt_pos_interface.registerHandle(ph_shoulder_joint);
hardware_interface::JointHandle ph_wrist_gripper_connection_pitch(jnt_state_interface.getHandle("wrist_gripper_connection_pitch"), &cmd[1]);
jnt_pos_interface.registerHandle(ph_wrist_gripper_connection_pitch);
hardware_interface::JointHandle ph_wrist_gripper_connection_roll(jnt_state_interface.getHandle("wrist_gripper_connection_roll"), &cmd[2]);
jnt_pos_interface.registerHandle(ph_wrist_gripper_connection_roll);
hardware_interface::JointHandle ph_shoulder_updown(jnt_state_interface.getHandle("shoulder_updown"), &cmd[3]);
jnt_pos_interface.registerHandle(ph_shoulder_updown);
hardware_interface::JointHandle ph_elbow(jnt_state_interface.getHandle("elbow"), &cmd[4]);
jnt_pos_interface.registerHandle(ph_elbow);
hardware_interface::JointHandle ph_wrist(jnt_state_interface.getHandle("wrist"), &cmd[5]);
jnt_pos_interface.registerHandle(ph_wrist);
hardware_interface::JointHandle ph_gripper(jnt_state_interface.getHandle("gripper"), &cmd[6]);
jnt_pos_interface.registerHandle(ph_gripper);
registerInterface(&jnt_pos_interface);
}
bool loadWGT()
{
MString filepath = wgtHome;
filepath += WGT_MAYA_PLUGIN_LIST_FILE;
std::vector<std::wstring> plugins;
if (!getWGTPlugins(plugins, filepath.asWChar()) || plugins.empty())
{
return MStatus::kFailure; // failed to find any plugins!
}
auto& contextManager = pluginManager->getContextManager();
contextManager.setExecutablePath(wgtHome);
auto globalContext = contextManager.getGlobalContext();
globalContext->registerInterface(new MemoryPluginContextCreator);
pluginManager->loadPlugins(plugins);
auto uiApp = globalContext->queryInterface<IUIApplication>();
if (!uiApp)
{
return false;
}
wgtApp = new ApplicationProxy(uiApp);
wgtApp->start();
auto automation = globalContext->queryInterface<IAutomation>();
if (automation != nullptr)
{
MGlobal::executeCommandOnIdle("evalDeferred(\"quit -force\")");
}
return true;
}
void TaurobClawNode::init()
{
ROS_INFO("reading parameters...");
// get parameters -- try to, at least
try
{
nh_.param<bool>("watchdog", watchdog, true);
ROS_INFO("Safeguard (Watchdog or Command RX Timeout): %s", (char*)(watchdog ? "Watchdog" : "Command RX Timeout"));
nh_.param<double>("rot_factor", rot_factor, 1.0);
nh_.param<double>("rot_offset", rot_offset, 0.0);
nh_.param<double>("grip_factor", grip_factor, 1.0);
nh_.param<double>("grip_offset", grip_offset, 0.0);
ROS_INFO("Factor: rot: %f, grip: %f", rot_factor, grip_factor);
ROS_INFO("Offsets: rot: %f, grip: %f", rot_offset, grip_offset);
}
catch(...)
{
ROS_ERROR("No configuration found -- won't be doing anything.");
}
//sub_jointstate = nh_->subscribe("jointstate_cmd", 1, &jointstate_cmd_callback);
sub_watchdog = nh_.subscribe("watchdog_feed", 1, &TaurobClawNode::watchdog_feed_callback, this);
sub_enabler = nh_.subscribe("enable_control", 1, &TaurobClawNode::enable_control_callback, this);
//pub_jointstate = nh_->advertise<sensor_msgs::JointState>("jointstate_status", 1);
//pub_force = nh_.advertise<std_msgs::Float64>("claw_force", 1);
hardware_interface::JointStateHandle state_handle_rot(JOINT_ROTAION, &joint_position_rotation, &joint_velocity_rotation, &joint_effort_rotation);
joint_state_interface_.registerHandle(state_handle_rot);
hardware_interface::JointHandle pos_handle_rot(joint_state_interface_.getHandle(JOINT_ROTAION), &joint_pos_cmd_rotation);
position_joint_interface_.registerHandle(pos_handle_rot);
hardware_interface::JointStateHandle state_handle_grip(JOINT_GRIP, &joint_position_grip, &joint_velocity_grip, &joint_effort_grip);
joint_state_interface_.registerHandle(state_handle_grip);
hardware_interface::JointHandle pos_handle_grip(joint_state_interface_.getHandle(JOINT_GRIP), &joint_pos_cmd_grip);
position_joint_interface_.registerHandle(pos_handle_grip);
registerInterface(&joint_state_interface_);
registerInterface(&position_joint_interface_);
nh_.param<double>("rot_init_pos", joint_pos_cmd_rotation, 0.0);
nh_.param<double>("grip_init_pos", joint_pos_cmd_grip, 0.0);
ROS_INFO("Initialization complete");
}
bool MitsubishiArmInterface::init(hardware_interface::JointStateInterface & jnt_state_interface_,
hardware_interface::PositionJointInterface & jnt_pos_interface_)
{
// connect and register the joint state interface
hardware_interface::JointStateHandle state_handle_j1("j1", &pos[0], &vel[0], &eff[0]);
jnt_state_interface_.registerHandle(state_handle_j1);
hardware_interface::JointStateHandle state_handle_j2("j2", &pos[1], &vel[1], &eff[1]);
jnt_state_interface_.registerHandle(state_handle_j2);
hardware_interface::JointStateHandle state_handle_j3("j3", &pos[2], &vel[2], &eff[2]);
jnt_state_interface_.registerHandle(state_handle_j3);
hardware_interface::JointStateHandle state_handle_j4("j4", &pos[3], &vel[3], &eff[3]);
jnt_state_interface_.registerHandle(state_handle_j4);
hardware_interface::JointStateHandle state_handle_j5("j5", &pos[4], &vel[4], &eff[4]);
jnt_state_interface_.registerHandle(state_handle_j5);
hardware_interface::JointStateHandle state_handle_j6("j6", &pos[5], &vel[5], &eff[5]);
jnt_state_interface_.registerHandle(state_handle_j6);
registerInterface(&jnt_state_interface_);
// connect and register the joint position interface
hardware_interface::JointHandle pos_handle_j1(jnt_state_interface_.getHandle("j1"), &cmd[0]);
jnt_pos_interface_.registerHandle(pos_handle_j1);
hardware_interface::JointHandle pos_handle_j2(jnt_state_interface_.getHandle("j2"), &cmd[1]);
jnt_pos_interface_.registerHandle(pos_handle_j2);
hardware_interface::JointHandle pos_handle_j3(jnt_state_interface_.getHandle("j3"), &cmd[2]);
jnt_pos_interface_.registerHandle(pos_handle_j3);
hardware_interface::JointHandle pos_handle_j4(jnt_state_interface_.getHandle("j4"), &cmd[3]);
jnt_pos_interface_.registerHandle(pos_handle_j4);
hardware_interface::JointHandle pos_handle_j5(jnt_state_interface_.getHandle("j5"), &cmd[4]);
jnt_pos_interface_.registerHandle(pos_handle_j5);
hardware_interface::JointHandle pos_handle_j6(jnt_state_interface_.getHandle("j6"), &cmd[5]);
jnt_pos_interface_.registerHandle(pos_handle_j6);
registerInterface(&jnt_pos_interface_);
}
IfabotHardware::IfabotHardware() : node_handle(), data_valid(false) {
ros::V_string joint_names = boost::assign::list_of("front_left_wheel")("front_right_wheel")("rear_left_wheel")("rear_right_wheel");
for(unsigned i = 0; i < JOINT_COUNT; i++) {
cmd[i] = 0.0;
hardware_interface::JointStateHandle joint_state_handle(joint_names[i],&pos[i],&vel[i],&eff[i]);
joint_state_interface.registerHandle(joint_state_handle);
hardware_interface::JointHandle joint_handle(joint_state_handle, &cmd[i]);
velocity_joint_interface.registerHandle(joint_handle);
}
registerInterface(&joint_state_interface);
registerInterface(&velocity_joint_interface);
velocity_command_publisher = node_handle.advertise<geometry_msgs::Twist>("robot_board_driver/cmd_vel",1000);
mSub = node_handle.subscribe("robot_board_driver/joint_state", 1000, &IfabotHardware::jointStatesReceivedCallback,this);
}
DummyProcessor::DummyProcessor(const swatch::core::AbstractStub& aStub) :
Processor(aStub),
mDriver(new DummyProcDriver())
{
// 1) Interfaces
registerInterface( new DummyTTC(*mDriver) );
registerInterface( new DummyReadoutInterface(*mDriver) );
registerInterface( new DummyAlgo(*mDriver) );
registerInterface( new processor::InputPortCollection() );
registerInterface( new processor::OutputPortCollection() );
const processor::ProcessorStub& stub = getStub();
for(auto it = stub.rxPorts.begin(); it != stub.rxPorts.end(); it++)
getInputPorts().addPort(new DummyRxPort(it->id, it->number, *mDriver));
for(auto it = stub.txPorts.begin(); it != stub.txPorts.end(); it++)
getOutputPorts().addPort(new DummyTxPort(it->id, it->number, *mDriver));
// 2) Commands
core::Command& reboot = registerCommand<DummyResetCommand>("reboot");
core::Command& reset = registerCommand<DummyResetCommand>("reset");
core::Command& cfgTx = registerCommand<DummyConfigureTxCommand>("configureTx");
core::Command& cfgRx = registerCommand<DummyConfigureRxCommand>("configureRx");
core::Command& cfgDaq = registerCommand<DummyConfigureDaqCommand>("configureDaq");
core::Command& cfgAlgo = registerCommand<DummyConfigureAlgoCommand>("configureAlgo");
registerCommand<DummyProcessorForceClkTtcStateCommand>("forceClkTtcState");
registerCommand<DummyProcessorForceRxPortsStateCommand>("forceRxPortsState");
registerCommand<DummyProcessorForceTxPortsStateCommand>("forceTxPortsState");
registerCommand<DummyProcessorForceReadoutStateCommand>("forceReadoutState");
registerCommand<DummyProcessorForceAlgoStateCommand>("forceAlgoState");
// 3) Command sequences
core::CommandSequence& cfgSeq = registerSequence("configPartA", reset).then(cfgDaq).then(cfgTx);
registerSequence("fullReconfigure", reset).then(cfgDaq).then(cfgAlgo).then(cfgRx).then(cfgTx);
// 4) State machines
processor::RunControlFSM& lFSM = getRunControlFSM();
lFSM.coldReset.add(reboot);
lFSM.setup.add(cfgSeq);
lFSM.configure.add(cfgAlgo);
lFSM.align.add(cfgRx);
lFSM.fsm.addTransition("dummyNoOp", processor::RunControlFSM::kStateAligned, processor::RunControlFSM::kStateInitial);
}
DummyAMC13Manager::DummyAMC13Manager( const swatch::core::AbstractStub& aStub ) :
dtm::DaqTTCManager(aStub),
mDriver(new DummyAMC13Driver())
{
// 0) Monitoring interfaces
registerInterface( new AMC13TTC(*mDriver) );
registerInterface( new AMC13SLinkExpress(0, *mDriver) );
registerInterface( new dtm::AMCPortCollection() );
for( uint32_t s(1); s<=kNumAMCPorts; ++s)
getAMCPorts().addPort(new AMC13BackplaneDaqPort(s, *mDriver));
registerInterface( new AMC13EventBuilder(*mDriver));
// 1) Commands
core::Command& reboot = registerCommand<DummyAMC13RebootCommand>("reboot");
core::Command& reset = registerCommand<DummyAMC13ResetCommand>("reset");
core::Command& cfgEvb = registerCommand<DummyAMC13ConfigureEvbCommand>("configureEvb");
core::Command& cfgSLink = registerCommand<DummyAMC13ConfigureSLinkCommand>("configureSLink");
core::Command& cfgAMCPorts = registerCommand<DummyAMC13ConfigureAMCPortsCommand>("configureAMCPorts");
core::Command& startDaq = registerCommand<DummyAMC13StartDaqCommand>("startDaq");
core::Command& stopDaq = registerCommand<DummyAMC13StopDaqCommand>("stopDaq");
registerCommand<DummyAMC13ForceClkTtcStateCommand>("forceClkTtcState");
registerCommand<DummyAMC13ForceEvbStateCommand>("forceEventBuilderState");
registerCommand<DummyAMC13ForceSLinkStateCommand>("forceSLinkState");
registerCommand<DummyAMC13ForceAMCPortStateCommand>("forceAMCPortState");
// 2) Command sequences
//registerFunctionoid<DaqTTCMgrCommandSequence>("resetAndConfigure").run(reset).then(configureDaq);
registerSequence("fullReconfigure", reboot).then(reset).then(cfgEvb).then(cfgSLink).then(cfgAMCPorts).then(startDaq);
// 3) State machines
dtm::RunControlFSM& lFSM = getRunControlFSM();
lFSM.coldReset.add(reboot);
lFSM.clockSetup.add(reset);
lFSM.cfgDaq.add(cfgEvb).add(cfgSLink).add(cfgAMCPorts);
lFSM.start.add(startDaq);
//lFSM.pause;
//lFSM.resume;
lFSM.stopFromPaused.add(stopDaq);
lFSM.stopFromRunning.add(stopDaq);
}
UART::UART(const char *name) : IService(name) {
registerInterface(static_cast<IMemoryOperation *>(this));
registerInterface(static_cast<ISerial *>(this));
registerAttribute("BaseAddress", &baseAddress_);
registerAttribute("Length", &length_);
registerAttribute("IrqLine", &irqLine_);
registerAttribute("IrqControl", &irqctrl_);
baseAddress_.make_uint64(0);
length_.make_uint64(0);
irqLine_.make_uint64(0);
irqctrl_.make_string("");
listeners_.make_list(0);
memset(®s_, 0, sizeof(regs_));
regs_.status = UART_STATUS_TX_EMPTY | UART_STATUS_RX_EMPTY;
p_rx_wr_ = rxfifo_;
p_rx_rd_ = rxfifo_;
rx_total_ = 0;
}
UdpService::UdpService(const char *name)
: IService(name) {
registerInterface(static_cast<IUdp *>(this));
registerAttribute("Timeout", &timeout_);
registerAttribute("BlockingMode", &blockmode_);
registerAttribute("HostIP", &hostIP_);
registerAttribute("BoardIP", &boardIP_);
timeout_.make_int64(0);
blockmode_.make_boolean(true);
hostIP_.make_string("192.168.0.53");
boardIP_.make_string("192.168.0.51");
}
/*
This method is "over generalized" to allow us to (potentially) register more types of things in
the future without adding exported symbols.
*/
int QQmlPrivate::qmlregister(RegistrationType type, void *data)
{
if (type == TypeRegistration) {
return registerType(*reinterpret_cast<RegisterType *>(data));
} else if (type == InterfaceRegistration) {
return registerInterface(*reinterpret_cast<RegisterInterface *>(data));
} else if (type == AutoParentRegistration) {
return registerAutoParentFunction(*reinterpret_cast<RegisterAutoParent *>(data));
} else if (type == SingletonRegistration) {
return registerSingletonType(*reinterpret_cast<RegisterSingletonType *>(data));
}
return -1;
}
SocInfo::SocInfo(const char *name)
: IService(name) {
registerInterface(static_cast<ISocInfo *>(this));
registerAttribute("PnpBaseAddress", &pnpBase_);
registerAttribute("DsuBaseAddress", &dsuBase_);
registerAttribute("GpioBaseAddress", &gpioBase_);
registerAttribute("ListCSR", &listCSR_);
registerAttribute("ListRegs", &listRegs_);
dsuBase_.make_uint64(0);
listCSR_.make_list(0);
listRegs_.make_list(0);
}
void buttonRegister()
{
int numberOfMessages;
int numberOfHandlers;
TCX_REG_MSG_TYPE messages[] = { BUTTONS_messages };
numberOfMessages = sizeof(messages)/sizeof(TCX_REG_MSG_TYPE);
numberOfHandlers =
sizeof(BUTTONS_reply_handler_array)/sizeof(TCX_REG_HND_TYPE);
registerInterface("", numberOfMessages, messages,
numberOfHandlers, BUTTONS_reply_handler_array);
}
static void doClassDeclaration(void)
{
PPARSEINFO pParseInfo=(PPARSEINFO)gScanner->user_data;
PINTERFACE pif;
gchar *chrTemp=pParseInfo->pCurInterface->chrName;
/* Check if we already have a (maybe forward) declaration */
pif=findInterfaceFromName(pParseInfo->pCurInterface->chrName);
if(pif)
{
if(pif->fIsForwardDeclaration)
{
/* Remove the forward declaration and insert the real thing afterwards. */
deRegisterInterface(pif);
}
else
{
/* It˚s the declaration from the *.h file. Save a pointer to this information. */
pParseInfo->pClassDefinition=pif;
deRegisterInterface(pif);
}
}
pParseInfo->pCurInterface->chrName=chrTemp;
pParseInfo->pCurInterface->chrSourceFileName=g_strdup(pParseInfo->chrCurrentSourceFile);
/* It's save to register the interface right here even if the struct is almost empty.
If anything goes wrong later we will exit anyway. */
registerInterface();
/* The class definition in *.nom files does not contain all the stuff an interface may define. We use the found
interface to fill the gaps. If we don˚t have an interface something went wrong and we quit. */
if(!pParseInfo->pClassDefinition)
{
g_message("Line %d: Error during class parsing. No class definition found. MAke sure you included the *.ih file.", g_scanner_cur_line(gScanner));
cleanupAndExit(0);
}
pParseInfo->pCurInterface->chrParent=g_strdup(pParseInfo->pClassDefinition->chrParent);
/* Note: We don˚t support subclasses yet. */
if(matchNext(':'))
{
parseParentClassIdent();
}
parseClassBody();
}
void
colliRegister()
{
int numberOfMessages;
int numberOfHandlers;
int i;
TCX_REG_MSG_TYPE messages[] = { BASE_messages };
numberOfMessages = sizeof(messages)/sizeof(TCX_REG_MSG_TYPE);
numberOfHandlers =
sizeof(BASE_reply_handler_array)/sizeof(TCX_REG_HND_TYPE);
registerInterface("", numberOfMessages, messages,
numberOfHandlers, BASE_reply_handler_array);
}
void
speechRegister()
{
/* OK to register these even if we do not use them */
int numberOfMessages;
TCX_REG_MSG_TYPE messages[] = SPEECH_MESSAGES;
if (bRobot.speech_type &&
(!strcmp(bRobot.speech_type, "doubleTalkPC") ||
!strcmp(bRobot.speech_type, "doubleTalkLT"))) {
SILENT = FALSE;
numberOfMessages = sizeof(messages)/sizeof(TCX_REG_MSG_TYPE);
registerInterface(SPEECH_SERVER_NAME, numberOfMessages,messages, 0, NULL);
}
else {
SILENT = TRUE;
}
}
static void doForwardClassDeclaration(void)
{
PPARSEINFO pParseInfo=(PPARSEINFO)gScanner->user_data;
PINTERFACE pif;
/* Check if we already have a (maybe forward) declaration */
pif=findInterfaceFromName(pParseInfo->pCurInterface->chrName);
if(pif)
{
/* One forward declaration is enough... */
g_free(pParseInfo->pCurInterface->chrName);
g_free(pParseInfo->pCurInterface);
}
else
{
pParseInfo->pCurInterface->chrSourceFileName=g_strdup(pParseInfo->chrCurrentSourceFile);
pParseInfo->pCurInterface->fIsForwardDeclaration=TRUE;
/* It's save to register the interface right here even if the struct is almost empty.
If anything goes wrong later we will exit anyway. */
registerInterface();
}
}
bool DefaultRobotHWSim::initSim(
const std::string& robot_namespace,
ros::NodeHandle model_nh,
gazebo::physics::ModelPtr parent_model,
const urdf::Model *const urdf_model,
std::vector<transmission_interface::TransmissionInfo> transmissions)
{
// getJointLimits() searches joint_limit_nh for joint limit parameters. The format of each
// parameter's name is "joint_limits/<joint name>". An example is "joint_limits/axle_joint".
const ros::NodeHandle joint_limit_nh(model_nh, robot_namespace);
// Resize vectors to our DOF
n_dof_ = transmissions.size();
joint_names_.resize(n_dof_);
joint_types_.resize(n_dof_);
joint_lower_limits_.resize(n_dof_);
joint_upper_limits_.resize(n_dof_);
joint_effort_limits_.resize(n_dof_);
joint_control_methods_.resize(n_dof_);
pid_controllers_.resize(n_dof_);
joint_position_.resize(n_dof_);
joint_velocity_.resize(n_dof_);
joint_effort_.resize(n_dof_);
joint_effort_command_.resize(n_dof_);
joint_position_command_.resize(n_dof_);
joint_velocity_command_.resize(n_dof_);
// Initialize values
for(unsigned int j=0; j < n_dof_; j++)
{
// Check that this transmission has one joint
if(transmissions[j].joints_.size() == 0)
{
ROS_WARN_STREAM_NAMED("default_robot_hw_sim","Transmission " << transmissions[j].name_
<< " has no associated joints.");
continue;
}
else if(transmissions[j].joints_.size() > 1)
{
ROS_WARN_STREAM_NAMED("default_robot_hw_sim","Transmission " << transmissions[j].name_
<< " has more than one joint. Currently the default robot hardware simulation "
<< " interface only supports one.");
continue;
}
std::vector<std::string> joint_interfaces = transmissions[j].joints_[0].hardware_interfaces_;
if (joint_interfaces.empty() &&
!(transmissions[j].actuators_.empty()) &&
!(transmissions[j].actuators_[0].hardware_interfaces_.empty()))
{
// TODO: Deprecate HW interface specification in actuators in ROS J
joint_interfaces = transmissions[j].actuators_[0].hardware_interfaces_;
ROS_WARN_STREAM_NAMED("default_robot_hw_sim", "The <hardware_interface> element of tranmission " <<
transmissions[j].name_ << " should be nested inside the <joint> element, not <actuator>. " <<
"The transmission will be properly loaded, but please update " <<
"your robot model to remain compatible with future versions of the plugin.");
}
if (joint_interfaces.empty())
{
ROS_WARN_STREAM_NAMED("default_robot_hw_sim", "Joint " << transmissions[j].joints_[0].name_ <<
" of transmission " << transmissions[j].name_ << " does not specify any hardware interface. " <<
"Not adding it to the robot hardware simulation.");
continue;
}
else if (joint_interfaces.size() > 1)
{
ROS_WARN_STREAM_NAMED("default_robot_hw_sim", "Joint " << transmissions[j].joints_[0].name_ <<
" of transmission " << transmissions[j].name_ << " specifies multiple hardware interfaces. " <<
"Currently the default robot hardware simulation interface only supports one. Using the first entry!");
//continue;
}
// Add data from transmission
joint_names_[j] = transmissions[j].joints_[0].name_;
joint_position_[j] = 1.0;
joint_velocity_[j] = 0.0;
joint_effort_[j] = 1.0; // N/m for continuous joints
joint_effort_command_[j] = 0.0;
joint_position_command_[j] = 0.0;
joint_velocity_command_[j] = 0.0;
const std::string& hardware_interface = joint_interfaces.front();
// Debug
ROS_DEBUG_STREAM_NAMED("default_robot_hw_sim","Loading joint '" << joint_names_[j]
<< "' of type '" << hardware_interface << "'");
// Create joint state interface for all joints
js_interface_.registerHandle(hardware_interface::JointStateHandle(
joint_names_[j], &joint_position_[j], &joint_velocity_[j], &joint_effort_[j]));
// Decide what kind of command interface this actuator/joint has
hardware_interface::JointHandle joint_handle;
if(hardware_interface == "EffortJointInterface")
{
// Create effort joint interface
joint_control_methods_[j] = EFFORT;
joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]),
&joint_effort_command_[j]);
ej_interface_.registerHandle(joint_handle);
}
else if(hardware_interface == "PositionJointInterface")
{
// Create position joint interface
joint_control_methods_[j] = POSITION;
joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]),
&joint_position_command_[j]);
pj_interface_.registerHandle(joint_handle);
}
else if(hardware_interface == "VelocityJointInterface")
{
// Create velocity joint interface
joint_control_methods_[j] = VELOCITY;
joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]),
&joint_velocity_command_[j]);
vj_interface_.registerHandle(joint_handle);
}
else
{
ROS_FATAL_STREAM_NAMED("default_robot_hw_sim","No matching hardware interface found for '"
<< hardware_interface );
return false;
}
// Get the gazebo joint that corresponds to the robot joint.
//ROS_DEBUG_STREAM_NAMED("default_robot_hw_sim", "Getting pointer to gazebo joint: "
// << joint_names_[j]);
gazebo::physics::JointPtr joint = parent_model->GetJoint(joint_names_[j]);
if (!joint)
{
ROS_ERROR_STREAM("This robot has a joint named \"" << joint_names_[j]
<< "\" which is not in the gazebo model.");
return false;
}
sim_joints_.push_back(joint);
registerJointLimits(joint_names_[j], joint_handle, joint_control_methods_[j],
joint_limit_nh, urdf_model,
&joint_types_[j], &joint_lower_limits_[j], &joint_upper_limits_[j],
&joint_effort_limits_[j]);
if (joint_control_methods_[j] != EFFORT)
{
// Initialize the PID controller. If no PID gain values are found, use joint->SetAngle() or
// joint->SetVelocity() to control the joint.
const ros::NodeHandle nh(model_nh, robot_namespace + "/gazebo_ros_control/pid_gains/" +
joint_names_[j]);
if (pid_controllers_[j].init(nh, true))
{
switch (joint_control_methods_[j])
{
case POSITION:
joint_control_methods_[j] = POSITION_PID;
break;
case VELOCITY:
joint_control_methods_[j] = VELOCITY_PID;
break;
}
}
else
{
// joint->SetMaxForce() must be called if joint->SetAngle() or joint->SetVelocity() are
// going to be called. joint->SetMaxForce() must *not* be called if joint->SetForce() is
// going to be called.
//joint->SetMaxForce(0, joint_effort_limits_[j]);
joint->SetParam("max_force", 0, joint_effort_limits_[j]);
}
}
}
// Register interfaces
registerInterface(&js_interface_);
registerInterface(&ej_interface_);
registerInterface(&pj_interface_);
registerInterface(&vj_interface_);
// Initialize the emergency stop code.
e_stop_active_ = false;
last_e_stop_active_ = false;
return true;
}
HF_HW_ros::HF_HW_ros(ros::NodeHandle &nh, std::string url, std::string config_addr) :
hf_hw_(url, config_addr),
nh_(nh)
{
//get the parameter
nh_.setCallbackQueue(&queue_);
base_mode_ = "4omni-wheel";
with_arm_ = false;
controller_freq_ = 100;
nh_.getParam("base_mode", base_mode_);
nh_.getParam("with_arm", with_arm_);
nh_.getParam("freq", controller_freq_);
robot_state_publisher_ = nh_.advertise<handsfree_msgs::robot_state>("robot_state", 10);
x_ = y_ = theta_ = x_cmd_ = y_cmd_ = theta_cmd_ = 0.0;
x_vel_ = y_vel_ = theta_vel_ = 0.0;
head_servo1_cmd_ = head_servo2_cmd_ = 0.0;
head_servo1_pos_ = head_servo1_vel_ = head_servo1_eff_ = 0;
head_servo2_pos_ = head_servo2_vel_ = head_servo2_eff_ = 0;
//register the hardware interface on the robothw
hardware_interface::BaseStateHandle base_state_handle("mobile_base", &x_, &y_, &theta_, &x_vel_, &y_vel_, &theta_vel_);
base_state_interface_.registerHandle(base_state_handle);
registerInterface(&base_state_interface_);
hardware_interface::BaseVelocityHandle base_handle(base_state_handle, &x_cmd_, &y_cmd_, &theta_cmd_);
base_velocity_interface_.registerHandle(base_handle);
registerInterface(&base_velocity_interface_);
if (base_mode_ == "3omni-wheel")
{
wheel_pos_.resize(3,0);
wheel_vel_.resize(3.0);
wheel_eff_.resize(3,0);
wheel_cmd_.resize(3,0);
hardware_interface::JointStateHandle wheel1_state_handle("wheel1_joint", &wheel_pos_[0], &wheel_vel_[0], &wheel_eff_[0]);
jnt_state_interface_.registerHandle(wheel1_state_handle);
hardware_interface::JointHandle wheel1_handle(wheel1_state_handle, &wheel_cmd_[0]);
base_vel_interface_.registerHandle(wheel1_handle);
hardware_interface::JointStateHandle wheel2_state_handle("wheel2_joint", &wheel_pos_[1], &wheel_vel_[1], &wheel_eff_[1]);
jnt_state_interface_.registerHandle(wheel2_state_handle);
hardware_interface::JointHandle wheel2_handle(wheel2_state_handle, &wheel_cmd_[1]);
base_vel_interface_.registerHandle(wheel2_handle);
hardware_interface::JointStateHandle wheel3_state_handle("wheel3_joint", &wheel_pos_[2], &wheel_vel_[2], &wheel_eff_[2]);
jnt_state_interface_.registerHandle(wheel3_state_handle);
hardware_interface::JointHandle wheel3_handle(wheel3_state_handle, &wheel_cmd_[2]);
base_vel_interface_.registerHandle(wheel3_handle);
registerInterface(&jnt_state_interface_);
registerInterface(&base_vel_interface_);
} else if (base_mode_ == "2diff-wheel")
{
wheel_pos_.resize(2,0);
wheel_vel_.resize(2.0);
wheel_eff_.resize(2,0);
wheel_cmd_.resize(2,0);
hardware_interface::JointStateHandle wheel1_state_handle("wheel1_joint", &wheel_pos_[0], &wheel_vel_[0], &wheel_eff_[0]);
jnt_state_interface_.registerHandle(wheel1_state_handle);
hardware_interface::JointHandle wheel1_handle(wheel1_state_handle, &wheel_cmd_[0]);
base_vel_interface_.registerHandle(wheel1_handle);
hardware_interface::JointStateHandle wheel2_state_handle("wheel2_joint", &wheel_pos_[1], &wheel_vel_[1], &wheel_eff_[1]);
jnt_state_interface_.registerHandle(wheel2_state_handle);
hardware_interface::JointHandle wheel2_handle(wheel2_state_handle, &wheel_cmd_[1]);
base_vel_interface_.registerHandle(wheel2_handle);
registerInterface(&jnt_state_interface_);
registerInterface(&base_vel_interface_);
} else if (base_mode_ == "4omni-wheel")
{
wheel_pos_.resize(4,0);
wheel_vel_.resize(4.0);
wheel_eff_.resize(4,0);
wheel_cmd_.resize(4,0);
hardware_interface::JointStateHandle wheel1_state_handle("wheel1_joint", &wheel_pos_[0], &wheel_vel_[0], &wheel_eff_[0]);
jnt_state_interface_.registerHandle(wheel1_state_handle);
hardware_interface::JointHandle wheel1_handle(wheel1_state_handle, &wheel_cmd_[0]);
base_vel_interface_.registerHandle(wheel1_handle);
hardware_interface::JointStateHandle wheel2_state_handle("wheel2_joint", &wheel_pos_[1], &wheel_vel_[1], &wheel_eff_[1]);
jnt_state_interface_.registerHandle(wheel2_state_handle);
hardware_interface::JointHandle wheel2_handle(wheel2_state_handle, &wheel_cmd_[1]);
base_vel_interface_.registerHandle(wheel2_handle);
hardware_interface::JointStateHandle wheel3_state_handle("wheel3_joint", &wheel_pos_[2], &wheel_vel_[2], &wheel_eff_[2]);
jnt_state_interface_.registerHandle(wheel3_state_handle);
hardware_interface::JointHandle wheel3_handle(wheel3_state_handle, &wheel_cmd_[2]);
base_vel_interface_.registerHandle(wheel3_handle);
hardware_interface::JointStateHandle wheel4_state_handle("wheel4_joint", &wheel_pos_[3], &wheel_vel_[3], &wheel_eff_[3]);
jnt_state_interface_.registerHandle(wheel4_state_handle);
hardware_interface::JointHandle wheel4_handle(wheel4_state_handle, &wheel_cmd_[3]);
base_vel_interface_.registerHandle(wheel4_handle);
registerInterface(&jnt_state_interface_);
registerInterface(&base_vel_interface_);
}
if (with_arm_)
{
for (int i = 0;i < 6;i++)
{
//get the joint name
std::stringstream ss;
ss << "arm" << (i + 1)<<std::endl;
hardware_interface::JointStateHandle arm_state_handle(ss.str(), &arm_pos_[i], &arm_pos_[i], &arm_pos_[i]);
jnt_state_interface_.registerHandle(arm_state_handle);
hardware_interface::JointHandle arm_handle(arm_state_handle , &arm_cmd_[i]);
servo_pos_interface_.registerHandle(arm_handle);
}
}
hardware_interface::JointStateHandle head_servo1_state_handle("pitch_joint", &head_servo1_pos_, &head_servo1_vel_, &head_servo1_eff_);
jnt_state_interface_.registerHandle(head_servo1_state_handle);
hardware_interface::JointHandle head_servo1_handle(head_servo1_state_handle, &head_servo1_cmd_);
servo_pos_interface_.registerHandle(head_servo1_handle);
hardware_interface::JointStateHandle head_servo2_state_handle("yaw_joint", &head_servo2_pos_, &head_servo2_vel_, &head_servo2_eff_);
jnt_state_interface_.registerHandle(head_servo2_state_handle);
hardware_interface::JointHandle head_servo2_handle(head_servo2_state_handle, &head_servo2_cmd_);
servo_pos_interface_.registerHandle(head_servo2_handle);
registerInterface(&jnt_state_interface_);
registerInterface(&servo_pos_interface_);
if (hf_hw_.initialize_ok())
{
ROS_INFO("system initialized succeed, ready for communication");
} else
{
ROS_ERROR("hf link initialized failed, please check the serial port of the openre board,for details,please see: http://wiki.hfreetech.org/");
}
}
MyRobot()
{
cmd[0]=0;
cmd[1]=0;
pos[0]=0;
pos[1]=0;
vel[0]=0;
vel[1]=0;
eff[0]=0;
eff[1]=0;
// connect and register the joint state interface
hardware_interface::JointStateHandle state_handle_a("JointA", &pos[0], &vel[0], &eff[0]); //pos vel eff outputs of the state message...
jnt_state_interface.registerHandle(state_handle_a);
hardware_interface::JointStateHandle state_handle_b("JointB", &pos[1], &vel[1], &eff[1]); //pos vel eff outputs of the state message...
jnt_state_interface.registerHandle(state_handle_b);
hardware_interface::JointHandle pos_handle_a(jnt_state_interface.getHandle("JointA"), &cmd[0]); //cmd is the commanded value depending on the controller.
hardware_interface::JointHandle pos_handle_b(jnt_state_interface.getHandle("JointB"), &cmd[1]); //cmd is the commanded value depending on the controller.
registerInterface(&jnt_state_interface);
hardware_interface::ImuSensorHandle::Data data;
data.name="ImuTest";
data.frame_id="Imu";
data.orientation=orientation;
data.orientation_covariance=orientation_covariance;
data.angular_velocity=&angular_velocity;
data.angular_velocity_covariance=angular_velocity_covariance;
data.linear_acceleration=&linear_acceleration;
data.linear_acceleration_covariance=linear_acceleration_covariance;
orientation_covariance[0]=1;
orientation_covariance[1]=2;
orientation_covariance[2]=3;
orientation_covariance[3]=4;
hardware_interface::ImuSensorHandle sensor_handle_imu(data);
imu_interface.registerHandle(sensor_handle_imu);
// connect and register the joint position interface
jnt_pos_interface.registerHandle(pos_handle_a);
jnt_vel_interface.registerHandle(pos_handle_a);
jnt_eff_interface.registerHandle(pos_handle_a);
jnt_pos_interface.registerHandle(pos_handle_b);
jnt_vel_interface.registerHandle(pos_handle_b);
jnt_eff_interface.registerHandle(pos_handle_b);
registerInterface(&jnt_pos_interface);
registerInterface(&jnt_vel_interface);
registerInterface(&jnt_eff_interface);
registerInterface(&imu_interface);
}
// Set up the data for ros_controllers
int32_t VigirHumanoidHWInterface::init_robot_controllers(boost::shared_ptr<vigir_control::VigirRobotModel>& robot_model, //boost::shared_ptr< std::vector<std::string> >& joint_list,
boost::shared_ptr<ros::NodeHandle>& behavior_control_nh,
boost::shared_ptr<ros::NodeHandle>& joint_control_nh,
boost::shared_ptr<ros::NodeHandle>& private_nh)
{
ROS_INFO(" Initialize the generic humanoid HW interfaces ...");
// Store the list of controlled joints
joint_names_ = robot_model->joint_names_;
try {
// State inputs
std::cout << "Initialize HW interface for " << joint_names_->size() << " joints!" << std::endl;
joint_state_positions_ = vigir_control::VectorNd::Constant(joint_names_->size(), 0.0);//.resize( joint_names_->size());
joint_state_velocities_ = vigir_control::VectorNd::Constant(joint_names_->size(), 0.0);//.resize(joint_names_->size());
joint_state_accelerations_ = vigir_control::VectorNd::Constant(joint_names_->size(), 0.0);//.resize(joint_names_->size());
joint_state_efforts_ = vigir_control::VectorNd::Constant(joint_names_->size(), 0.0);//.resize(joint_names_->size());
// Control outputs
joint_command_positions_ = vigir_control::VectorNd::Constant(joint_names_->size(), 0.0);
joint_command_velocities_ = vigir_control::VectorNd::Constant(joint_names_->size(), 0.0);
joint_command_accelerations_ = vigir_control::VectorNd::Constant(joint_names_->size(), 0.0);
joint_command_control_ = vigir_control::VectorNd::Constant(joint_names_->size(), 0.0);
joint_command_efforts_ = vigir_control::VectorNd::Constant(joint_names_->size(), 0.0);
joint_command_friction_compensation_ = vigir_control::VectorNd::Constant(joint_names_->size(), 0.0);
joint_position_errors_ = vigir_control::VectorNd::Constant(joint_names_->size(), 0.0);
joint_velocity_errors_ = vigir_control::VectorNd::Constant(joint_names_->size(), 0.0);
joint_effort_errors_ = vigir_control::VectorNd::Constant(joint_names_->size(), 0.0);
}
catch(...)
{
std::cerr << "Failed to allocate memory for controllers!" << std::endl;
return ROBOT_CONTROLLERS_FAILED_TO_INITIALIZE;
}
for (size_t i = 0; i < joint_names_->size(); ++i){
hardware_interface::JointStateHandle state_handle(joint_names_->at(i), &joint_state_positions_[i], &joint_state_velocities_[i], &joint_state_efforts_[i]);
joint_state_interface_.registerHandle(state_handle);
hardware_interface::JointHandle pos_handle(joint_state_interface_.getHandle(joint_names_->at(i)), &joint_command_positions_[i]);
position_joint_interface_.registerHandle(pos_handle);
hardware_interface::JointHandle vel_handle(joint_state_interface_.getHandle(joint_names_->at(i)), &joint_command_velocities_[i]);
velocity_joint_interface_.registerHandle(vel_handle);
hardware_interface::JointHandle effort_handle(joint_state_interface_.getHandle(joint_names_->at(i)), &joint_command_efforts_[i]);
effort_joint_interface_.registerHandle(effort_handle);
hardware_interface::PosVelAccJointHandle pos_vel_acc_handle (joint_state_interface_.getHandle(joint_names_->at(i)),
&joint_command_positions_[i], &joint_command_velocities_[i], &joint_command_accelerations_[i]);
pos_vel_acc_joint_interface_.registerHandle(pos_vel_acc_handle);
hardware_interface::PosVelAccErrHumanoidJointHandle handle (joint_state_interface_.getHandle(joint_names_->at(i)),
&joint_command_positions_[i], &joint_command_velocities_[i], &joint_command_accelerations_[i],
&joint_position_errors_[i], &joint_velocity_errors_[i]);
pos_vel_acc_err_humanoid_joint_interface_.registerHandle(handle);
}
try {
ROS_INFO(" Load the whole robot controller handle ...");
hardware_interface::VigirHumanoidControllerHandle controller_handle(std::string("controller_handle"),
&joint_state_positions_,
&joint_state_velocities_,
&joint_state_accelerations_,
&joint_state_efforts_,
&joint_command_positions_, //!< desired position
&joint_command_velocities_, //!< desired velocity
&joint_command_accelerations_, //!< desired acceleration
&joint_command_efforts_, //!< desired effort
&joint_command_control_, //!< desired control command (acceleration)
&joint_command_friction_compensation_, //!< effort to compensate for friction
&joint_position_errors_,
&joint_velocity_errors_,
&joint_effort_errors_,
&robot_l_foot_wrench_,
&robot_r_foot_wrench_,
&robot_l_hand_wrench_,
&robot_r_hand_wrench_,
&robot_pose_,
robot_model);
humanoid_controller_interface_.registerHandle(controller_handle);
ROS_INFO(" Register the controller interface...");
registerInterface(&humanoid_controller_interface_);
}
catch(...)
{
ROS_ERROR("Exception: could not initialize the Atlas pelvis control interface");
return ROBOT_EXCEPTION_CONTROLLERS_FAILED_TO_INITIALIZE;
}
registerInterface(&joint_state_interface_);
registerInterface(&position_joint_interface_);
registerInterface(&velocity_joint_interface_);
registerInterface(&effort_joint_interface_);
registerInterface(&pos_vel_acc_joint_interface_);
registerInterface(&pos_vel_acc_err_humanoid_joint_interface_);
ROS_INFO(" Done initializing the base HWInterface class for the humanoid controller ...");
return ROBOT_INITIALIZED_OK;
}
void LWRHW::registerInterfaces(const urdf::Model *const urdf_model,
std::vector<transmission_interface::TransmissionInfo> transmissions)
{
// Check that this transmission has one joint
if( transmissions.empty() )
{
std::cout << "lwr_hw: " << "There are no transmission in this robot, all are non-driven joints? "
<< std::endl;
return;
}
// Initialize values
for(int j=0; j < n_joints_; j++)
{
// Check that this transmission has one joint
if(transmissions[j].joints_.size() == 0)
{
std::cout << "lwr_hw: " << "Transmission " << transmissions[j].name_
<< " has no associated joints." << std::endl;
continue;
}
else if(transmissions[j].joints_.size() > 1)
{
std::cout << "lwr_hw: " << "Transmission " << transmissions[j].name_
<< " has more than one joint, and they can't be controlled simultaneously"
<< std::endl;
continue;
}
std::vector<std::string> joint_interfaces = transmissions[j].joints_[0].hardware_interfaces_;
if( joint_interfaces.empty() )
{
std::cout << "lwr_hw: " << "Joint " << transmissions[j].joints_[0].name_ <<
" of transmission " << transmissions[j].name_ << " does not specify any hardware interface. " <<
"You need to, otherwise the joint can't be controlled." << std::endl;
continue;
}
const std::string& hardware_interface = joint_interfaces.front();
// Debug
std::cout << "\x1B[37m" << "lwr_hw: " << "Loading joint '" << joint_names_[j]
<< "' of type '" << hardware_interface << "'" << "\x1B[0m" << std::endl;
// Create joint state interface for all joints
state_interface_.registerHandle(hardware_interface::JointStateHandle(
joint_names_[j], &joint_position_[j], &joint_velocity_[j], &joint_effort_[j]));
// Decide what kind of command interface this actuator/joint has
hardware_interface::JointHandle joint_handle_effort;
joint_handle_effort = hardware_interface::JointHandle(state_interface_.getHandle(joint_names_[j]),
&joint_effort_command_[j]);
effort_interface_.registerHandle(joint_handle_effort);
hardware_interface::JointHandle joint_handle_position;
joint_handle_position = hardware_interface::JointHandle(state_interface_.getHandle(joint_names_[j]),
&joint_position_command_[j]);
position_interface_.registerHandle(joint_handle_position);
// the stiffness is not actually a different joint, so the state handle is only used for handle
hardware_interface::JointHandle joint_handle_stiffness;
joint_handle_stiffness = hardware_interface::JointHandle(hardware_interface::JointStateHandle(
joint_names_[j]+std::string("_stiffness"),
&joint_stiffness_[j], &joint_stiffness_[j], &joint_stiffness_[j]),
&joint_stiffness_command_[j]);
position_interface_.registerHandle(joint_handle_stiffness);
// velocity command handle, recall it is fake, there is no actual velocity interface
hardware_interface::JointHandle joint_handle_velocity;
joint_handle_velocity = hardware_interface::JointHandle(state_interface_.getHandle(joint_names_[j]),
&joint_velocity_command_[j]);
registerJointLimits(joint_names_[j],
joint_handle_effort,
joint_handle_position,
joint_handle_velocity,
joint_handle_stiffness,
urdf_model,
&joint_lower_limits_[j], &joint_upper_limits_[j],
&joint_lower_limits_stiffness_[j],
&joint_upper_limits_stiffness_[j],
&joint_effort_limits_[j]);
}
// Register interfaces
registerInterface(&state_interface_);
registerInterface(&effort_interface_);
registerInterface(&position_interface_);
}
void GenericHWInterface::init()
{
ROS_INFO_STREAM_NAMED("generic_hw_interface", "Reading rosparams from namespace: " << nh_.getNamespace());
// Get joint names
nh_.getParam("hardware_interface/joints", joint_names_);
if (joint_names_.size() == 0)
{
ROS_FATAL_STREAM_NAMED("generic_hw_interface",
"No joints found on parameter server for controller, did you load the proper yaml file?"
<< " Namespace: " << nh_.getNamespace() << "/hardware_interface/joints");
exit(-1);
}
num_joints_ = joint_names_.size();
// Status
joint_position_.resize(num_joints_, 0.0);
joint_velocity_.resize(num_joints_, 0.0);
joint_effort_.resize(num_joints_, 0.0);
// Command
joint_position_command_.resize(num_joints_, 0.0);
joint_velocity_command_.resize(num_joints_, 0.0);
joint_effort_command_.resize(num_joints_, 0.0);
// Limits
joint_position_lower_limits_.resize(num_joints_, 0.0);
joint_position_upper_limits_.resize(num_joints_, 0.0);
joint_velocity_limits_.resize(num_joints_, 0.0);
joint_effort_limits_.resize(num_joints_, 0.0);
// Initialize interfaces for each joint
for (std::size_t joint_id = 0; joint_id < num_joints_; ++joint_id)
{
ROS_DEBUG_STREAM_NAMED("generic_hw_interface", "Loading joint name: " << joint_names_[joint_id]);
// Create joint state interface
joint_state_interface_.registerHandle(hardware_interface::JointStateHandle(
joint_names_[joint_id], &joint_position_[joint_id], &joint_velocity_[joint_id], &joint_effort_[joint_id]));
// Add command interfaces to joints
// TODO: decide based on transmissions?
hardware_interface::JointHandle joint_handle_position = hardware_interface::JointHandle(
joint_state_interface_.getHandle(joint_names_[joint_id]), &joint_position_command_[joint_id]);
position_joint_interface_.registerHandle(joint_handle_position);
hardware_interface::JointHandle joint_handle_velocity = hardware_interface::JointHandle(
joint_state_interface_.getHandle(joint_names_[joint_id]), &joint_velocity_command_[joint_id]);
velocity_joint_interface_.registerHandle(joint_handle_velocity);
hardware_interface::JointHandle joint_handle_effort = hardware_interface::JointHandle(
joint_state_interface_.getHandle(joint_names_[joint_id]), &joint_effort_command_[joint_id]);
effort_joint_interface_.registerHandle(joint_handle_effort);
// Load the joint limits
registerJointLimits(joint_handle_position, joint_handle_velocity, joint_handle_effort, joint_id);
} // end for each joint
registerInterface(&joint_state_interface_); // From RobotHW base class.
registerInterface(&position_joint_interface_); // From RobotHW base class.
registerInterface(&velocity_joint_interface_); // From RobotHW base class.
registerInterface(&effort_joint_interface_); // From RobotHW base class.
}
scara_setup::ScaraSetupHWA::ScaraSetupHWA()
{
trans[0] = 1.0; //linear, no transmission, this is handled by the velocity control loop
trans[1] = 2.0; //shoulder
trans[2] = -2.0; //elbow
trans[3] = -2.0; //<- third joint is the wrist
trans[4] = -1.0; //fingerjoint
reset_switch = false;
// connect and register the joint state interface
hardware_interface::JointStateHandle state_handle_linear("linear", &jnt_pos[0], &jnt_vel[0], &jnt_eff[0]);
jnt_state_interface.registerHandle(state_handle_linear);
hardware_interface::JointStateHandle state_handle_shoulder("shoulder", &jnt_pos[1], &jnt_vel[1], &jnt_eff[1]);
jnt_state_interface.registerHandle(state_handle_shoulder);
hardware_interface::JointStateHandle state_handle_elbow("elbow", &jnt_pos[2], &jnt_vel[2], &jnt_eff[2]);
jnt_state_interface.registerHandle(state_handle_elbow);
hardware_interface::JointStateHandle state_handle_wrist("wrist", &jnt_pos[3], &jnt_vel[3], &jnt_eff[3]);
jnt_state_interface.registerHandle(state_handle_wrist);
hardware_interface::JointStateHandle state_handle_fingerjoint("fingerjoint", &jnt_pos[4], &jnt_vel[4], &jnt_eff[4]);
jnt_state_interface.registerHandle(state_handle_fingerjoint);
registerInterface(&jnt_state_interface);
// connect and register the joint position interface
hardware_interface::JointHandle pos_handle_linear(jnt_state_interface.getHandle("linear"), &jnt_cmd[0]);
jnt_pos_interface.registerHandle(pos_handle_linear);
hardware_interface::JointHandle pos_handle_shoulder(jnt_state_interface.getHandle("shoulder"), &jnt_cmd[1]);
jnt_pos_interface.registerHandle(pos_handle_shoulder);
hardware_interface::JointHandle pos_handle_elbow(jnt_state_interface.getHandle("elbow"), &jnt_cmd[2]);
jnt_pos_interface.registerHandle(pos_handle_elbow);
hardware_interface::JointHandle pos_handle_wrist(jnt_state_interface.getHandle("wrist"), &jnt_cmd[3]);
jnt_pos_interface.registerHandle(pos_handle_wrist);
hardware_interface::JointHandle pos_handle_fingerjoint(jnt_state_interface.getHandle("fingerjoint"), &jnt_cmd[4]);
jnt_pos_interface.registerHandle(pos_handle_fingerjoint);
registerInterface(&jnt_pos_interface);
//set up the publishers & listeners
linear_cmd_pub = n.advertise<std_msgs::Float64>("/full_hw_controller/linear/command", 1000);
linear_state_sub = n.subscribe("/scara_setup/linear_encoder/value", 1000, &scara_setup::ScaraSetupHWA::linearCb, this);
shoulder_cmd_pub = n.advertise<std_msgs::Float64>("/full_hw_controller/shoulder/command", 1000);
shoulder_state_sub = n.subscribe("/full_hw_controller/shoulder/state", 1000, &scara_setup::ScaraSetupHWA::shoulderCb, this);
elbow_cmd_pub = n.advertise<std_msgs::Float64>("/full_hw_controller/elbow/command", 1000);
elbow_state_sub = n.subscribe("/full_hw_controller/elbow/state", 1000, &scara_setup::ScaraSetupHWA::elbowCb, this);
wrist_cmd_pub = n.advertise<std_msgs::Float64>("/full_hw_controller/wrist/command", 1000);
wrist_state_sub = n.subscribe("/full_hw_controller/wrist/state", 1000, &scara_setup::ScaraSetupHWA::wristCb, this);
fingerjoint_cmd_pub = n.advertise<std_msgs::Float64>("/full_hw_controller/fingerjoint/command", 1000);
fingerjoint_state_sub = n.subscribe("/full_hw_controller/fingerjoint/state", 1000, &scara_setup::ScaraSetupHWA::fingerjointCb, this);
reset_positions_sub = n.subscribe("/scara_setup/reset_positions", 1000, &scara_setup::ScaraSetupHWA::resetPositionsCb, this);
}
void OpenMPBLAS3::setInterface( BLASInterface& BLAS )
{
// Note: macro takes advantage of same name for routines and type definitions
// ( e.g. routine CUDABLAS1::sum<T> is set for BLAS::BLAS1::sum variable
LAMA_INTERFACE_REGISTER_T( BLAS, gemm, float )
LAMA_INTERFACE_REGISTER_T( BLAS, gemm, double )
// trsm routines are not used yet by LAMA
}
/* --------------------------------------------------------------------------- */
/* Static registration of the BLAS3 routines */
/* --------------------------------------------------------------------------- */
bool OpenMPBLAS3::registerInterface()
{
LAMAInterface& interface = LAMAInterfaceRegistry::getRegistry().modifyInterface( Context::Host );
setInterface( interface.BLAS );
return true;
}
/* --------------------------------------------------------------------------- */
/* Static initialiazion at program start */
/* --------------------------------------------------------------------------- */
bool OpenMPBLAS3::initialized = registerInterface();
} /* namespace lama */
// Register all interfaces necessary
void PanTiltHW::registerInterfaces_(const urdf::Model *const urdf_model,
std::vector<transmission_interface::TransmissionInfo> transmissions)
{
// Check that this transmission has one joint
if( transmissions.empty() )
{
ROS_INFO("PanTiltHW: There are no transmission in this robot, all are non-driven joints ?");
return;
}
// Initialize values
for(int j=0; j < n_joints_; j++)
{
#if TRACE_ACTIVATED
traceTransmissionInfo_(transmissions[j]);
#endif
// Check that this transmission has one joint
if(transmissions[j].joints_.size() == 0)
{
ROS_INFO("PanTiltHW: Transmission %s has no associated joints.",transmissions[j].name_.c_str());
continue;
}
else if(transmissions[j].joints_.size() > 1)
{
ROS_INFO("PanTiltHW: Transmission %s has more than one joint, and they can't be controlled simultaneously.",transmissions[j].name_.c_str());
continue;
}
std::vector<std::string> joint_interfaces = transmissions[j].joints_[0].hardware_interfaces_;
if( joint_interfaces.empty() )
{
ROS_INFO("PanTiltHW: Joint %s of transmission %s does not specify any hardware interface. You need to, otherwise the joint can't be controlled.", transmissions[j].joints_[0].name_.c_str(), transmissions[j].name_.c_str());
continue;
}
const std::string& hardware_interface = joint_interfaces.front();
#if TRACE_ACTIVATED
ROS_INFO("PanTiltHW: Loading joint '%s', of hardware interface type '%s'", joint_names_[j].c_str(), hardware_interface.c_str());
#endif
// JOINT STATE **********************************************************************************************************************
// Create a joint state handle for one joint and register it in a JointStateInterface.
#if TRACE_ACTIVATED
ROS_INFO("PanTiltHW: register 'hardware_interface::JointStateHandle' of joint %s", joint_names_[j].c_str());
#endif
// hardware_interface::JointStateHandle -> JointStateHandle(const std::string& name, const double* pos, const double* vel, const double* eff)
// hardware_interface::ResourceManager -> void registerHandle(const ResourceHandle& handle)
// hardware_interface::JointStateInterface : Hardware interface to support reading the state of an array of joints.
joint_state_interface_.registerHandle(hardware_interface::JointStateHandle(joint_names_[j], &joint_position_[j], &joint_velocity_[j], &joint_effort_[j]));
// EFFORT **********************************************************************************************************************
// hardware_interface::EffortJointInterface : for commanding effort-based joints.
#if TRACE_ACTIVATED
ROS_INFO("PanTiltHW: register 'hardware_interface::EffortJointInterface' of joint %s", joint_names_[j].c_str());
#endif
hardware_interface::JointHandle joint_handle_effort;
joint_handle_effort = hardware_interface::JointHandle(joint_state_interface_.getHandle(joint_names_[j]),
&joint_effort_command_[j]);
joint_effort_interface_.registerHandle(joint_handle_effort);
// POSITION **********************************************************************************************************************
// hardware_interface::JointHandle : A handle used to read and command a single joint.
// hardware_interface::PositionJointInterface : for commanding position-based joints.
#if TRACE_ACTIVATED
ROS_INFO("PanTiltHW: register 'hardware_interface::PositionJointInterface' of joint %s", joint_names_[j].c_str());
#endif
hardware_interface::JointHandle joint_handle_position;
joint_handle_position = hardware_interface::JointHandle(joint_state_interface_.getHandle(joint_names_[j]),
&joint_position_command_[j]);
joint_position_interface_.registerHandle(joint_handle_position);
}
// Register joints state, effort, position interfaces to 'hardware_interface::RobotHW'
registerInterface(&joint_state_interface_);
registerInterface(&joint_effort_interface_);
registerInterface(&joint_position_interface_);
}
