public void OnRosConnected()
{
Bridge.AddService <Ros.Srv.Int, Ros.Srv.Int>(buzzerTopicName, msg =>
{
if (msg.data == 0)
{
SetBuzzerMode(BuzzerModeTypes.BuzzerOff);
}
else if (msg.data == 1)
{
SetBuzzerMode(BuzzerModeTypes.BuzzerOne);
}
else if (msg.data == 2)
{
SetBuzzerMode(BuzzerModeTypes.BuzzerTwo);
}
else if (msg.data == 3)
{
SetBuzzerMode(BuzzerModeTypes.BuzzerThree);
}
return(new Ros.Srv.Int()
{
data = 1
});
});
}
public void OnRosConnected()
{
if (TargetRosEnv == ROSTargetEnvironment.APOLLO)
{
Bridge.AddPublisher <Ros.Apollo.Imu>(ImuTopic);
Bridge.AddPublisher <Ros.CorrectedImu>(ApolloIMUOdometryTopic);
}
else if (TargetRosEnv == ROSTargetEnvironment.AUTOWARE || TargetRosEnv == ROSTargetEnvironment.DUCKIETOWN_ROS1)
{
Bridge.AddPublisher <Ros.Imu>(ImuTopic);
Bridge.AddPublisher <Ros.Odometry>(OdometryTopic);
Bridge.AddService <Ros.Srv.Empty, Ros.Srv.Empty>(ResetOdometry, msg =>
{
odomPosition = new Vector3(0f, 0f, 0f);
return(new Ros.Srv.Empty());
});
}
}
public void OnRosConnected()
{
Bridge.AddService <Ros.Srv.Int, Ros.Srv.Int>(topLightTopicName, msg =>
{
switch (msg.data)
{
case 0:
SetTopLightMode(false);
break;
case 1:
SetTopLightMode(true);
break;
}
return(new Ros.Srv.Int()
{
data = 1
});
});
}
public void OnRosConnected()
{
Bridge.AddService <Ros.Srv.String, Ros.Srv.String>(ledServiceName, msg =>
{
var response = new Ros.Srv.String();
if (msg.str == null || msg.str.Length == 0)
{
response.str = "Invalid input!";
return(response);
}
switch (msg.str[0])
{
case 'c':
SetLEDMode(LEDModeTypes.None);
break;
case 'f':
SetLEDMode(LEDModeTypes.Fade);
break;
case 'b':
SetLEDMode(LEDModeTypes.Blink);
break;
case 'a':
SetLEDMode(LEDModeTypes.All);
break;
case 'r':
SetLEDMode(LEDModeTypes.Right);
break;
case 'l':
SetLEDMode(LEDModeTypes.Left);
break;
default:
response.str += "Invalid code for LED mode! ";
break;
}
if (msg.str.Length > 1)
{
switch (msg.str[1])
{
case 'g':
SetLEDColor(LEDColorTypes.Green);
break;
case 'r':
SetLEDColor(LEDColorTypes.Red);
break;
case 'b':
SetLEDColor(LEDColorTypes.Blue);
break;
case 'w':
SetLEDColor(LEDColorTypes.White);
break;
case 'o':
SetLEDColor(LEDColorTypes.Orange);
break;
case 'R':
SetLEDColor(LEDColorTypes.Rainbow);
break;
default:
response.str += "Invalid code for LED color!";
return(response);
}
}
if (response.str == null)
{
response.str = "LED color/mode changed";
}
return(response);
});
}
public void OnRosConnected()
{
Bridge.AddService <Ros.Srv.Empty, Ros.Srv.Empty>(CENTER_GRIPPER_SRV, msg =>
{
hook.CenterHook();
return(new Ros.Srv.Empty());
});
Bridge.AddService <Ros.Srv.SetBool, Ros.Srv.SetBoolResponse>(ATTACH_GRIPPER_SRV, msg =>
{
hook.EngageHook(msg.data);
return(new Ros.Srv.SetBoolResponse()
{
success = true, message = ""
});
});
// tugbot
Bridge.Subscribe(CMD_VEL_TOPIC,
(Ros.Twist msg) =>
{
float WHEEL_SEPARATION = 0.515f;
float WHEEL_DIAMETER = 0.39273163f;
float SCALING_RATIO = 0.208f;
// Assuming that we only get linear in x and angular in z
double v = msg.linear.x;
double w = msg.angular.z;
wheelLeftVel = SCALING_RATIO * (float)(v - w * 0.5 * WHEEL_SEPARATION);
wheelRightVel = SCALING_RATIO * (float)(v + w * 0.5 * WHEEL_SEPARATION);
});
Bridge.Subscribe(WHEEL_CMD_TOPIC,
(WheelsCmdStampedMsg msg) =>
{
wheelLeftVel = msg.vel_left;
wheelRightVel = msg.vel_right;
});
// Autoware vehicle command
Bridge.Subscribe(AUTOWARE_CMD_TOPIC,
(Ros.VehicleCmd msg) =>
{
float WHEEL_SEPARATION = 0.1044197f;
//float WHEEL_DIAMETER = 0.065f;
float L_GAIN = 0.25f;
float A_GAIN = 8.0f;
// Assuming that we only get linear in x and angular in z
double v = msg.twist_cmd.twist.linear.x;
double w = msg.twist_cmd.twist.angular.z;
wheelLeftVel = (float)(L_GAIN * v - A_GAIN * w * 0.5 * WHEEL_SEPARATION);
wheelRightVel = (float)(L_GAIN * v + A_GAIN * w * 0.5 * WHEEL_SEPARATION);
});
string override_topic;
if (Bridge.Version == 1)
{
Bridge.AddPublisher <Ros.Joy>(JOYSTICK_ROS1);
override_topic = JOYSTICK_OVERRIDE_TOPIC_ROS1;
}
else
{
override_topic = JOYSTICK_OVERRIDE_TOPIC_ROS2;
}
Bridge.AddPublisher <BoolStamped>(override_topic);
Bridge.Subscribe <BoolStamped>(override_topic, stamped =>
{
ManualControl = stamped.data;
});
if (FirstConnection)
{
var stamp = new BoolStamped()
{
header = new Ros.Header()
{
stamp = Ros.Time.Now(),
seq = seq++,
frame_id = "joystick",
},
data = true,
};
var topic = (Bridge.Version == 1) ? JOYSTICK_OVERRIDE_TOPIC_ROS1 : JOYSTICK_OVERRIDE_TOPIC_ROS2;
Bridge.Publish(topic, stamp);
FirstConnection = false;
}
ManualControl = true;
}
