odcore::data::dmcp::ModuleExitCodeMessage::ModuleExitCode Gcdc2016::body()
{
odcore::data::TimeStamp previousTimestep;
while (getModuleStateAndWaitForRemainingTimeInTimeslice() ==
odcore::data::dmcp::ModuleStateMessage::RUNNING) {
odcore::data::TimeStamp thisTimestep;
odcore::data::TimeStamp duration = thisTimestep - previousTimestep;
previousTimestep = thisTimestep;
//std::cout << "vehicle heading: " << m_vehicle->GetHeading() << std::endl;
double deltaTime = duration.toMicroseconds() / 1000000.0;
//std::cout << "deltaTime: " << deltaTime << std::endl;
m_vehicle->Update(deltaTime);
m_vehicle->SetThrottlePedalPosition(30.0);
// double lateralPosition = m_vehicle->GetLateralPosition();
// double heading = m_vehicle->GetHeading();
double speed = m_vehicle->GetSpeed();
std::cout << "velocity: " << speed << std::endl;
// std::cout << "Heading (degrees): " << (heading * 57.295779513) << std::endl;
// std::cout << "Lateral position: " << lateralPosition << std::endl;
// TODO: Use Surface instead.
// opendlv::perception::LanePosition lanePosition(lateralPosition, heading);
// odcore::data::Container msg(lanePosition);
// getConference().send(msg);
}
return odcore::data::dmcp::ModuleExitCodeMessage::OKAY;
}
/**
* Receives .
* Sends .
*/
odcore::data::dmcp::ModuleExitCodeMessage::ModuleExitCode Identity::body()
{
while (getModuleStateAndWaitForRemainingTimeInTimeslice() ==
odcore::data::dmcp::ModuleStateMessage::RUNNING){
odcore::data::TimeStamp now;
opendlv::knowledge::Insight stationIdInsight(now,"stationId="+std::to_string(m_stationId));
SendContainer(stationIdInsight);
opendlv::knowledge::Insight stationTypeInsight(now,"stationType="+std::to_string(m_stationType));
SendContainer(stationTypeInsight);
opendlv::knowledge::Insight vehicleLengthInsight(now,"vehicleLength="+std::to_string(m_vehicleLength));
SendContainer(vehicleLengthInsight);
opendlv::knowledge::Insight vehicleWidthInsight(now,"vehicleWidth="+std::to_string(m_vehicleWidth));
SendContainer(vehicleWidthInsight);
opendlv::knowledge::Insight vehicleRoleInsight(now,"vehicleRole="+std::to_string(m_vehicleRole));
SendContainer(vehicleRoleInsight);
opendlv::knowledge::Insight rearAxleLocationInsight(now,"rearAxleLocation="+std::to_string(m_rearAxleLocation));
SendContainer(rearAxleLocationInsight);
}
return odcore::data::dmcp::ModuleExitCodeMessage::OKAY;
}
odcore::data::dmcp::ModuleExitCodeMessage::ModuleExitCode ProxyIMU::body() {
while (getModuleStateAndWaitForRemainingTimeInTimeslice() == odcore::data::dmcp::ModuleStateMessage::RUNNING) {
opendlv::proxy::AngularVelocityReading gyroscopeReading = m_device->ReadGyroscope();
odcore::data::Container gyroscopeContainer(gyroscopeReading);
getConference().send(gyroscopeContainer);
opendlv::proxy::AccelerationReading accelerometerReading = m_device->ReadAccelerometer();
odcore::data::Container accelerometerContainer(accelerometerReading);
getConference().send(accelerometerContainer);
opendlv::proxy::MagneticFieldReading magnetometerReading = m_device->ReadMagnetometer();
odcore::data::Container magnetometerContainer(magnetometerReading);
getConference().send(magnetometerContainer);
opendlv::proxy::AltitudeReading altimeterReading = m_device->ReadAltimeter();
odcore::data::Container altimeterContainer(altimeterReading);
getConference().send(altimeterContainer);
opendlv::proxy::TemperatureReading temperatureReading = m_device->ReadTemperature();
odcore::data::Container temperatureContainer(temperatureReading);
getConference().send(temperatureContainer);
if (m_debug) {
std::cout << gyroscopeReading.toString() << ", "
<< accelerometerReading.toString() << ", "
<< magnetometerReading.toString() << ", "
<< altimeterReading.toString() << ", "
<< temperatureReading.toString() << std::endl;
}
}
return odcore::data::dmcp::ModuleExitCodeMessage::OKAY;
}
odcore::data::dmcp::ModuleExitCodeMessage::ModuleExitCode SonarArray::body()
{
while (getModuleStateAndWaitForRemainingTimeInTimeslice()
== odcore::data::dmcp::ModuleStateMessage::RUNNING) {
auto sonarReadings = m_device->GetEchoReadings();
odcore::data::Container sonarReadingContainer(sonarReadings);
getConference().send(sonarReadingContainer);
}
return odcore::data::dmcp::ModuleExitCodeMessage::OKAY;
}
// This method will do the main data processing job.
odcore::data::dmcp::ModuleExitCodeMessage::ModuleExitCode Proxy::body() {
double vcAngle = -0.2;
int speed = 1530;
while (getModuleStateAndWaitForRemainingTimeInTimeslice() == odcore::data::dmcp::ModuleStateMessage::RUNNING) {
Container containerVehicleControl = getKeyValueDataStore().get(automotive::VehicleControl::ID());
VehicleControl vc = containerVehicleControl.getData<VehicleControl>();
// TODO implement as per example:
//vc.toString() results in Speed: 0 Acceleration: 0 SteeringWheelAngle: 0 BrakeLights: 0 FlashingLightsLeft: 0 FlashingLightsRight: 0
//double vcAngle = vc.getSteeringWheelAngle();
// use config file to set them
// Read config file only once in setup?
m_in_min= -0.2;
m_in_max = 0.2;
m_out_min = 60;
m_out_max = 120;
int arduinoSteerValue = Proxy::arduino_map(vcAngle);
int SPEED_FIXED_ARDUINO_VAL = speed;
std::string DELIM_KEY_VALUE = "=";
std::string DELIM_PAIR = ";";
std::string SPEED_KEY = "speed";
std::string ANGLE_KEY = "angle";
// -- end of constants
std::stringstream strStream;
std::string toBeSentString;
strStream << SPEED_KEY << DELIM_KEY_VALUE << SPEED_FIXED_ARDUINO_VAL << DELIM_PAIR;
strStream << ANGLE_KEY << DELIM_KEY_VALUE << arduinoSteerValue << DELIM_PAIR;
strStream >> toBeSentString;
m_nsp->send(toBeSentString);
//vcAngle = vcAngle + 0.05;
const uint32_t ONE_SECOND = 1000 * 1000;
odcore::base::Thread::usleepFor(10 * ONE_SECOND);
}
return odcore::data::dmcp::ModuleExitCodeMessage::OKAY;
}
odcore::data::dmcp::ModuleExitCodeMessage::ModuleExitCode Ivrule::body()
{
while (getModuleStateAndWaitForRemainingTimeInTimeslice() == odcore::data::dmcp::ModuleStateMessage::RUNNING) {
odcore::data::TimeStamp now;
if((m_mioValidUntil-now).toMicroseconds() > 0) {
// Steer to mio
opendlv::perception::StimulusDirectionOfMovement sdom(now, m_mio.getDirection(),opendlv::model::Direction(0,0));
odcore::data::Container containerSdom(sdom);
getConference().send(containerSdom);
// std::cout << "Sent sdom." << std::endl;
opendlv::perception::StimulusAngularSizeAlignment sasa(now, m_mio.getDirection(),m_mio.getAngularSize(),m_desiredAngularSize);
odcore::data::Container containerSasa(sasa);
getConference().send(containerSasa);
// std::cout << "Sent sasa." << std::endl;
// std::cout
// << " Id: " << m_mio.getObjectId()
// << " Azimuth: " << m_mio.getDirection().getAzimuth()
// << " Distance: " << m_mio.getDistance()
// << " Desired angular size: " << m_desiredAngularSize
// << " Currently: " << m_mio.getAngularSize()
// << std::endl;
} else {
opendlv::perception::StimulusGroundSpeed sof(now, m_desiredOpticalFlow, m_speed);
odcore::data::Container containerSof(sof);
getConference().send(containerSof);
// std::cout << "Send sof. Desired OF: " << m_desiredOpticalFlow << " Current OF: " << m_speed << std::endl;
}
}
return odcore::data::dmcp::ModuleExitCodeMessage::OKAY;
}
