public override void _start_robot()
{
//TODO: remove this
_ros_node = new ROSNode();
_joint_states_sub = _ros_node.subscribe <ros_csharp_interop.rosmsg.gen.sensor_msgs.JointState>(_ros_ns_prefix + "robot/joint_states", 1, _joint_state_cb);;
_robot_state_sub = _ros_node.subscribe <AssemblyState>(_ros_ns_prefix + "robot/state", 1, _robot_state_cb);
if (_arm_selection != BaxterRobotArmSelection.right)
{
_endpoint_state_sub = _ros_node.subscribe <EndpointState>(_ros_ns_prefix + "robot/limb/left/endpoint_state", 1, x => _endpoint_state_cb(x, 0));
_joint_command_pub_left = _ros_node.advertise <JointCommand>(_ros_ns_prefix + "robot/limb/left/joint_command", 1, false);
}
if (_arm_selection != BaxterRobotArmSelection.left)
{
_endpoint_state_sub = _ros_node.subscribe <EndpointState>(_ros_ns_prefix + "robot/limb/right/endpoint_state", 1, x => _endpoint_state_cb(x, _arm_selection == BaxterRobotArmSelection.right ? 0 : 1));
_joint_command_pub_right = _ros_node.advertise <JointCommand>(_ros_ns_prefix + "robot/limb/right/joint_command", 1, false);
}
_set_super_reset_pub = _ros_node.advertise <Empty>(_ros_ns_prefix + "robot/set_super_reset", 1, false);
_set_super_stop_pub = _ros_node.advertise <Empty>(_ros_ns_prefix + "robot/set_super_stop", 1, false);
_set_super_enable_pub = _ros_node.advertise <Bool>(_ros_ns_prefix + "robot/set_super_enable", 1, false);
_digital_io_pub = _ros_node.advertise <DigitalOutputCommand>(_ros_ns_prefix + "robot/digital_io/command", 10, false);
foreach (var d in _digital_io_names)
{
var sub = _ros_node.subscribe <DigitalIOState>(_ros_ns_prefix + $"robot/digitial_io/{d}/state", 1, msg => _digitital_io_state_cb(d, msg));
}
base._start_robot();
_ros_node.start_spinner();
}
public override void _start_tool()
{
_ros_node = new ROSNode();
_gripper_state_sub = _ros_node.subscribe <IODeviceStatus>(_ros_ns_prefix + "io/end_effector/" + _ros_tool_name + "/state", 1, _gripper_state_cb);;
_gripper_command_pub = _ros_node.advertise <IOComponentCommand>(_ros_ns_prefix + "io/end_effector/" + _ros_tool_name + "/command", 1, false);
base._start_tool();
}
public override void _start_tool()
{
_ros_node = new ROSNode();
_gripper_state_sub = _ros_node.subscribe <EndEffectorState>(_ros_ns_prefix + "robot/end_effector/" + _ros_tool_name + "/state", 1, _gripper_state_cb);;
_gripper_command_pub = _ros_node.advertise <EndEffectorCommand>(_ros_ns_prefix + "robot/end_effector/" + _ros_tool_name + "/command", 1, false);
base._start_tool();
}
public override void _start_robot()
{
_operational_mode = RobotOperationalMode.cobot;
_ros_node = new ROSNode();
_joint_states_sub = _ros_node.subscribe <ros_csharp_interop.rosmsg.gen.sensor_msgs.JointState>(_ros_ns_prefix + "robot/joint_states", 1, _joint_state_cb);;
_robot_state_sub = _ros_node.subscribe <RobotAssemblyState>(_ros_ns_prefix + "robot/state", 1, _robot_state_cb);;
_endpoint_state_sub = _ros_node.subscribe <EndpointState>(_ros_ns_prefix + "robot/limb/right/endpoint_state", 1, _endpoint_state_cb);;
_joint_command_pub = _ros_node.advertise <JointCommand>(_ros_ns_prefix + "robot/limb/right/joint_command", 1, false);
_set_super_reset_pub = _ros_node.advertise <Empty>(_ros_ns_prefix + "robot/set_super_reset", 1, false);
_set_super_stop_pub = _ros_node.advertise <Empty>(_ros_ns_prefix + "robot/set_super_stop", 1, false);
_set_super_enable_pub = _ros_node.advertise <Bool>(_ros_ns_prefix + "robot/set_super_enable", 1, false);
_homing_command_pub = _ros_node.advertise <HomingCommand>(_ros_ns_prefix + "robot/set_homing_mode", 1, false);
_digital_io_pub = _ros_node.advertise <DigitalOutputCommand>(_ros_ns_prefix + "robot/digital_io/command", 10, false);
foreach (var d in _digital_io_names)
{
var sub = _ros_node.subscribe <DigitalIOState>(_ros_ns_prefix + $"robot/digital_io/{d}/state", 1, msg => _digitital_io_state_cb(d, msg));
_digital_io_sub.Add(sub);
}
base._start_robot();
_ros_node.start_spinner();
}
static public void Main(String[] args)
{
ros_csharp_interop.ros_csharp_interop.init_ros(args, "JointStateSanityTest", true);
using (var node = new ROSNode())
{
using (var sub = node.subscribe <JointState>("joint_states", 1, joint_states_cb))
using (pub = node.advertise <JointState>("joint_states2", 1, false))
{
node.start_spinner();
Console.WriteLine("Press enter to quit");
Console.ReadKey();
}
}
}