void setup()
{
nh.initNode();
nh.advertise(sensor_val);
nh.advertise(sensor_voltage);
nh.advertise(sensor_temp);
}
int main()
{
int error = 0;
t.start();
nh.initNode();
nh.advertise(pub_temp);
nh.advertise(pub_humidity);
long publisher_timer = 0;
temp_msg.header.frame_id = "/base_link";
humidity_msg.header.frame_id = "/base_link";
while (1)
{
if (t.read_ms() > publisher_timer)
{
error = sensor.readData();
if (0 == error)
{
temp_msg.temperature = sensor.ReadTemperature(CELCIUS);
temp_msg.header.stamp = nh.now();
pub_temp.publish(&temp_msg);
humidity_msg.relative_humidity = sensor.ReadHumidity();
humidity_msg.header.stamp = nh.now();
pub_humidity.publish(&humidity_msg);
}
publisher_timer = t.read_ms() + 1000;
}
nh.spinOnce();
}
}
bool QuadrotorHardwareSim::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)
{
ros::NodeHandle param_nh(model_nh, "controller");
// store parent model pointer
model_ = parent_model;
link_ = model_->GetLink();
physics_ = model_->GetWorld()->GetPhysicsEngine();
// subscribe state
std::string state_topic;
param_nh.getParam("state_topic", state_topic);
if (!state_topic.empty()) {
ros::SubscribeOptions ops = ros::SubscribeOptions::create<nav_msgs::Odometry>(state_topic, 1, boost::bind(&QuadrotorHardwareSim::stateCallback, this, _1), ros::VoidConstPtr(), &callback_queue_);
subscriber_state_ = model_nh.subscribe(ops);
gzlog << "[hector_quadrotor_controller_gazebo] Using topic '" << subscriber_state_.getTopic() << "' as state input for control" << std::endl;
} else {
gzlog << "[hector_quadrotor_controller_gazebo] Using ground truth from Gazebo as state input for control" << std::endl;
}
// subscribe imu
std::string imu_topic;
param_nh.getParam("imu_topic", imu_topic);
if (!imu_topic.empty()) {
ros::SubscribeOptions ops = ros::SubscribeOptions::create<sensor_msgs::Imu>(imu_topic, 1, boost::bind(&QuadrotorHardwareSim::imuCallback, this, _1), ros::VoidConstPtr(), &callback_queue_);
subscriber_imu_ = model_nh.subscribe(ops);
gzlog << "[hector_quadrotor_controller_gazebo] Using topic '" << subscriber_imu_.getTopic() << "' as imu input for control" << std::endl;
} else {
gzlog << "[hector_quadrotor_controller_gazebo] Using ground truth from Gazebo as imu input for control" << std::endl;
}
// subscribe motor_status
{
ros::SubscribeOptions ops = ros::SubscribeOptions::create<airdrone_gazebo::MotorStatus>("motor_status", 1, boost::bind(&QuadrotorHardwareSim::motorStatusCallback, this, _1), ros::VoidConstPtr(), &callback_queue_);
subscriber_motor_status_ = model_nh.subscribe(ops);
}
// publish wrench
{
ros::AdvertiseOptions ops = ros::AdvertiseOptions::create<geometry_msgs::WrenchStamped>("command/wrench", 1, ros::SubscriberStatusCallback(), ros::SubscriberStatusCallback(), ros::VoidConstPtr(), &callback_queue_);
publisher_wrench_command_ = model_nh.advertise(ops);
}
// publish motor command
{
ros::AdvertiseOptions ops = ros::AdvertiseOptions::create<airdrone_gazebo::MotorCommand>("command/motor", 1, ros::SubscriberStatusCallback(), ros::SubscriberStatusCallback(), ros::VoidConstPtr(), &callback_queue_);
publisher_motor_command_ = model_nh.advertise(ops);
}
return true;
}
// +++++++++++++++++++++++++ main loop +++++++++++++++++++++++++++++++++++++++++++
void setup()
{
// init
nh.initNode();
// setup
setupMotor();
setupSensor();
setupServo();
// advertise
nh.advertise(pub_sensor_tracks);
nh.advertise(pub_range);
// subscribe
nh.subscribe(subscriberServo);
nh.subscribe(motor_sub);
}
int main() {
unsigned long last_pub;
/* set up interrupt handling */
// set up timer interrupts
// fast PWM mode; interrupt and reset when counter equals OCR0A
// prescalar 64
TCCR0A = (1 << WGM01 | 1 << WGM00);
TCCR0B = (1 << WGM02 | 1 << CS01 | 1 << CS00);
// interrupt on "overflow" (counter match)
TIMSK0 = (1 << TOIE0);
OCR0A = 249; // 250 counts per tick
nh.initNode();
nh.advertise(pub);
nh.subscribe(sub);
last_pub = nh.now().toNsec();
while(1) {
nh.spinOnce();
// do our best to publish once per second
if( nh.now().toNsec() - last_pub > 1000000000ull ) {
pub.publish(&msg);
last_pub += 1000000000ull;
}
}
}
int main(void)
{
// TivaC application specific code
MAP_FPUEnable();
MAP_FPULazyStackingEnable();
// TivaC system clock configuration. Set to 80MHz.
MAP_SysCtlClockSet(SYSCTL_SYSDIV_2_5 | SYSCTL_USE_PLL | SYSCTL_XTAL_16MHZ | SYSCTL_OSC_MAIN);
uint8_t button_debounced_delta;
uint8_t button_raw_state;
ButtonsInit();
// ROS nodehandle initialization and topic registration
nh.initNode();
nh.advertise(button_publisher);
while (1)
{
uint8_t button_debounced_state = ButtonsPoll(&button_debounced_delta, &button_raw_state);
// Publish message to be transmitted.
button_msg.sw1.data = button_debounced_state & LEFT_BUTTON;
button_msg.sw2.data = button_debounced_state & RIGHT_BUTTON;
button_publisher.publish(&button_msg);
// Handle all communications and callbacks.
nh.spinOnce();
// Delay for a bit.
nh.getHardware()->delay(100);
}
}
int main()
{
volatile unsigned int d;
/* initialize ROS & subscribers & publishers */
//nh.initNode();
nh.initNode(rosSrvrIp);
nh.advertise(sonar1); // advertise sonar range topic
nh.subscribe(motorSub); // subscribe to motor speed topic
nh.subscribe(servoSub); // subscribe to servo position
// reset bit 0, set as output for sonar trigger
gpio.SetData(0x0000);
gpio.SetDataDirection(0x0001);
// set callbacks on negative edge for both bits 0 (trigger)
// and 1 (echo)
gpio.RegisterCallback(0, NULL, callback);
gpio.RegisterCallback(1, NULL, callback);
gpio.SetInterruptMode(0, QEG_INTERRUPT_NEGEDGE);
gpio.SetInterruptMode(1, QEG_INTERRUPT_NEGEDGE);
// trigger sonar by toggling bit 0
while(1)
{
gpio.SetData(0x0001);
for (d=0; d<120000; d++);
gpio.SetData(0x0000);
usleep(100000); // the interrupt breaks us out of this sleep
usleep(100000); // now really sleep
sonar1.publish( &range );
nh.spinOnce();
}
}
int main() {
t.start();
nh.initNode();
nh.advertise(pub_temp);
long publisher_timer =0;
while (1) {
if (t.read_ms() > publisher_timer) {
// step 1: request reading from sensor
//Wire.requestFrom(sensorAddress,2);
char cmd = 2;
i2c.write(sensorAddress, &cmd, 1);
wait_ms(50);
char msb;
char lsb;
int temperature;
i2c.read(sensorAddress, &msb, 1); // receive high byte (full degrees)
i2c.read(sensorAddress, &lsb, 1); // receive low byte (fraction degrees)
temperature = ((msb) << 4); // MSB
temperature |= (lsb >> 4); // LSB
temp_msg.data = temperature*0.0625;
pub_temp.publish(&temp_msg);
publisher_timer = t.read_ms() + 1000;
}
nh.spinOnce();
}
void setup()
{
nh.initNode();
pinMode(led, OUTPUT);
pinMode(directionPinEast1, OUTPUT);
pinMode(directionPinEast2, OUTPUT);
pinMode(pwmPinWest, OUTPUT);
pinMode(directionPinWest2, OUTPUT);
pinMode(pwmPinEast, OUTPUT);
pinMode(directionPinWest1, OUTPUT);
pinMode(directionPinSouthSway1, OUTPUT);
pinMode(directionPinSouthSway2, OUTPUT);
pinMode(pwmPinNorthSway, OUTPUT);
pinMode(directionPinNorthSway2, OUTPUT);
pinMode(pwmPinSouthSway, OUTPUT);
pinMode(directionPinNorthSway1, OUTPUT);
pinMode(directionPinSouthUp1, OUTPUT);
pinMode(directionPinSouthUp2, OUTPUT);
pinMode(pwmPinNorthUp, OUTPUT);
pinMode(directionPinNorthUp2, OUTPUT);
pinMode(pwmPinSouthUp, OUTPUT);
pinMode(directionPinNorthUp1, OUTPUT);
nh.subscribe(subPwmForward);
nh.subscribe(subPwmSideward);
nh.subscribe(subPwmUpward);
nh.subscribe(subPwmTurn);
nh.advertise(ps_voltage);
Serial.begin(57600);
}
ros::Publisher ShapeShifter::advertise(ros::NodeHandle& nh, const std::string& topic, uint32_t queue_size_, bool latch, const ros::SubscriberStatusCallback &connect_cb) const
{
ros::AdvertiseOptions opts(topic, queue_size_, getMD5Sum(), getDataType(), getMessageDefinition(), connect_cb);
opts.latch = latch;
return nh.advertise(opts);
}
/**
* @brief Initializes the gyro/accelerometer and the magnetometer unit of the imu.
* As well as the arduino subsystem
*/
void setup() {
Wire.begin();
delay(1500);
/********/
/* GYRO */
/********/
gyro.init();
gyro.writeReg(L3G_CTRL_REG4, 0x00); // 245 dps scale
gyro.writeReg(L3G_CTRL_REG1, 0x0F); // normal power mode, all axes enabled, 100 Hz
//8.75 mdps/LSB
/****************/
/* MAGNETOMETER */
/****************/
compass.init();
compass.enableDefault();
compass.writeReg(LSM303::CTRL2, 0x08); // 4 g scale: AFS = 001
//0.122 mg/LSB
compass.writeReg(LSM303::CTRL5, 0x10); // Magnetometer Low Resolution 50 Hz
//Magnetometer 4 gauss scale : 0.16mgauss/LSB
//ROS-TF base frame of the imu_data
imu_msg.header.frame_id="base_imu_link";
//Register ROS messages
nh.initNode();
nh.advertise(imu_pub);
nh.advertise(mag_pub);
//starting value for the timer
timer=millis();
}
// Note: connector labeled "INPUT" on sonar sensor goes to
// digital 1 (bit 0), and connector labeled "OUTPUT" goes to
// digital 2 (bit 1).
int main()
{
CQEGpioInt &gpio = CQEGpioInt::GetRef();
volatile unsigned int d;
// reset bit 0, set as output for sonar trigger
gpio.SetData(0x0000);
gpio.SetDataDirection(0x0001);
// set callbacks on negative edge for both bits 0 (trigger)
// and 1 (echo)
gpio.RegisterCallback(0, NULL, callback);
gpio.RegisterCallback(1, NULL, callback);
gpio.SetInterruptMode(0, QEG_INTERRUPT_NEGEDGE);
gpio.SetInterruptMode(1, QEG_INTERRUPT_NEGEDGE);
//nh.initNode();
nh.initNode(rosSrvrIp);
nh.advertise(sonar1);
// trigger sonar by toggling bit 0
while(1)
{
gpio.SetData(0x0001);
for (d=0; d<120000; d++);
gpio.SetData(0x0000);
sleep(1); // the interrupt breaks us out of this sleep
sleep(1); // now really sleep a second
sonar1.publish( &range );
nh.spinOnce();
}
}
void setup() {
// put your setup code here, to run once:
StepMotor.setSpeed(150);
pinMode(Button, INPUT_PULLUP);
pinMode(Sensor_Down, INPUT_PULLUP);
pinMode(Sensor_Up, INPUT_PULLUP);
pinMode(EnA, OUTPUT);
pinMode(EnB, OUTPUT);
nh.initNode();
nh.subscribe(sub);
nh.advertise(answer_pub);
nh.advertise(switch_pub);
if(digitalRead(Button)){
deadman_activated = true;
deadman_switch_.data = deadman_activated;
deadman_switch_.header.stamp = nh.now();
switch_pub.publish(&deadman_switch_);
}
else{
deadman_activated = false;
deadman_switch_.data = deadman_activated;
deadman_switch_.header.stamp = nh.now();
switch_pub.publish(&deadman_switch_);
}
down();
}
void setup(){
nh.initNode();
nh.advertise(pause_pub);
nh.subscribe(rate_sub);
pinMode(PAUSE_PIN, INPUT);
digitalWrite(PAUSE_PIN, HIGH);//enable pullup
pinMode(LIGHT_PIN, OUTPUT);
digitalWrite(LIGHT_PIN, LIGHT_ON);
}
void setup()
{
motor_VFL.attach(MOTOR_VFL_PIN);
motor_VFR.attach(MOTOR_VFR_PIN);
motor_VBL.attach(MOTOR_VBL_PIN);
motor_VBR.attach(MOTOR_VBR_PIN);
motor_HFL.attach(MOTOR_HFL_PIN);
motor_HFR.attach(MOTOR_HFR_PIN);
motor_HBL.attach(MOTOR_HBL_PIN);
motor_HBR.attach(MOTOR_HBR_PIN);
motor_VFL.writeMicroseconds(STOP_PWM);
motor_VFR.writeMicroseconds(STOP_PWM);
motor_VBL.writeMicroseconds(STOP_PWM);
motor_VBR.writeMicroseconds(STOP_PWM);
motor_HFL.writeMicroseconds(STOP_PWM);
motor_HFR.writeMicroseconds(STOP_PWM);
motor_HBL.writeMicroseconds(STOP_PWM);
motor_HBR.writeMicroseconds(STOP_PWM);
delay(1000);
Wire.begin();
sDepth.init();
sDepth.setFluidDensity(997);
pinMode(START_IN_PIN, INPUT_PULLUP);
pinMode(STOP_IN_PIN, INPUT_PULLUP);
nh.initNode();
nh.advertiseService(server2);
nh.advertise(pDepth);
nh.advertise(pStart);
nh.advertise(pStop);
nh.advertise(chatter);
nh.subscribe(sub);
}
void setup()
{
Wire.begin();
nh.initNode();
nh.subscribe(joint_1_set_setpoint_listener);
nh.advertise(joint_1_position_publisher);
nh.subscribe(joint_2_set_setpoint_listener);
nh.advertise(joint_2_position_publisher);
nh.subscribe(joint_3_set_setpoint_listener);
nh.advertise(joint_3_position_publisher);
joint_1_current_setpoint = 0.0; // Default setpoint
joint_1_new_setpoint = 0.0;
joint_2_current_setpoint = 0.0; // Default setpoint
joint_2_new_setpoint = 0.0;
joint_3_current_setpoint = 0.0; // Default setpoint
joint_3_new_setpoint = 0.0;
}
int main()
{
// Initialize ADC Interrupts
lastTimeInterruptLeft = avr_time_now();
lastTimeInterruptRight = avr_time_now();
out_msg.deltaUmLeft = 0;
out_msg.deltaUmRight = 0;
// Initialize ROS
nh.initNode();
nh.advertise(io_board_out);
ros::Subscriber<io_to_board> sub("to_ioboard", &ioboard_cb);
nh.subscribe(sub);
// Do timed/repeated stuff
uint32_t lastTimeOdometry = 0UL;
uint32_t lastTimeRake = 0UL;
while(1)
{
// Stop engines and raise rake, if last recieved message is older then 2s
// %TODO raise rake
if ((lastTimeMessage != 0) && (avr_time_now() - lastTimeMessage > 1000))
{
lastTimeMessage = 0;
OCR1A = 0x8000;
OCR1B = 0x8000;
}
// Publish odometry all 40ms
if (avr_time_now() - lastTimeOdometry > 40)
{
out_msg.timestamp = avr_time_now();
io_board_out.publish(&out_msg);
out_msg.deltaUmLeft = 0;
out_msg.deltaUmRight = 0;
lastTimeOdometry = avr_time_now();
}
nh.spinOnce();
// LUFA functions that need to be called frequently to keep USB alive
CDC_Device_USBTask(&Atmega32u4Hardware::VirtualSerial_CDC_Interface);
USB_USBTask();
}
return 0;
}
void setup()
{
nh.initNode();
nh.advertise(pub_range);
range_msg.radiation_type = sensor_msgs::Range::ULTRASOUND;
range_msg.header.frame_id = frameid;
range_msg.field_of_view = 0.1; // fake
range_msg.min_range = 0.0;
range_msg.max_range = 6.47;
pinMode(8,OUTPUT);
digitalWrite(8, LOW);
}
void setup()
{
pinMode(13, OUTPUT);
// Set up all of the Digital IO pins.
pinMode(pin_leftCutterCheck,INPUT);
pinMode(pin_rightCutterCheck,INPUT);
pinMode(pin_leftCutterControl,OUTPUT);
pinMode(pin_rightCutterControl,OUTPUT);
// Turn off the cutters by default
digitalWrite(pin_leftCutterControl,LOW);
digitalWrite(pin_rightCutterControl,LOW);
// Initialize the rear panel LED outputs
pinMode(pin_ledHigh,OUTPUT);
pinMode(pin_ledMid,OUTPUT);
pinMode(pin_ledLow,OUTPUT);
digitalWrite(pin_ledHigh, LOW);
digitalWrite(pin_ledMid, LOW);
digitalWrite(pin_ledLow, LOW);
temperatureTop.begin();
temperatureBot.begin();
// Make sure we have temperature sensors, if not, set to something
// unreasonable. This would be 0 in Alabama.
if(!temperatureTop.getAddress(topAddress,0))
{
msgStatus.temperature_1 = 0.0;
} else {
temperatureTop.setResolution(topAddress,9);
temperatureTop.setWaitForConversion(false);
temperatureTop.requestTemperatures();
}
if(!temperatureBot.getAddress(botAddress,0))
{
msgStatus.temperature_2 = 0.0;
} else {
temperatureBot.setResolution(botAddress,9);
temperatureBot.setWaitForConversion(false);
temperatureBot.requestTemperatures();
}
nh.initNode();
nh.advertise(status_pub);
nh.advertiseService(cutter_srv);
}
int main() {
nh.initNode();
nh.advertise(chatter);
while (1) {
str_msg.data = hello;
chatter.publish( &str_msg );
nh.logdebug(debug);
nh.loginfo(info);
nh.logwarn(warn);
nh.logerror(errors);
nh.logfatal(fatal);
nh.spinOnce();
wait_ms(500);
}
}
/*
* ROS rosserial publisher thread.
*/
msg_t rosserial_pub_thread(void * arg) {
std_msgs::String str_msg;
ros::Publisher pub("chatter", &str_msg);
(void) arg;
chRegSetThreadName("rosserial_pub");
nh.initNode();
nh.advertise(pub);
for (;;) {
char hello[] = "Hello world!";
str_msg.data = hello;
pub.publish(&str_msg);
nh.spinOnce();
chThdSleepMilliseconds(500);
}
return CH_SUCCESS;
}
void setup() {
nh.initNode();
last_time = nh.now();
current_time = nh.now();
//Serial.begin(38400);
//#####delay(1000);
rlog = new RosLogger(nh);
//#####quadratureEncoder = new QuadratureEncoder();
// lineSensor = new LineSensor();
// lineSensor->calibrate();
nh.advertise(odom_pub);
motor = new Motor(nh);
/*
Motor::Command c;
c.direction = Motor::STOP; motor->enqueue(c);
c.direction = Motor::BACKWARD; motor->enqueue(c);
c.direction = Motor::FORWARD; motor->enqueue(c);
c.direction = Motor::STOP; motor->enqueue(c);
c.direction = Motor::RIGHT_TURN; motor->enqueue(c);
c.direction = Motor::STOP; motor->enqueue(c);
c.direction = Motor::BACKWARD; motor->enqueue(c);
c.direction = Motor::STOP; motor->enqueue(c);
c.direction = Motor::LEFT_TURN; motor->enqueue(c);
c.direction = Motor::STOP; motor->enqueue(c);
*/
nh.subscribe(Motor::sub);
while (!nh.connected()) { nh.spinOnce(); }
rlog->info("START UP...");
}
int main(void)
{
SetSysClockTo56();
// ROS nodehandle initialization and topic registration
nh.initNode();
nh.advertise(chatter);
// Initialize LED
GPIO_InitTypeDef GPIO_Config;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
GPIO_Config.GPIO_Pin = GPIO_Pin_5;
GPIO_Config.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_Config.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOB, &GPIO_Config);
GPIO_WriteBit(GPIOB, GPIO_Pin_5, Bit_RESET);
while (1)
{
// Toggle LED
if (GPIO_ReadOutputDataBit(GPIOB, GPIO_Pin_5))
GPIO_WriteBit(GPIOB, GPIO_Pin_5, Bit_RESET);
else
GPIO_WriteBit(GPIOB, GPIO_Pin_5, Bit_SET);
// Publish message to be transmitted.
str_msg.data = hello;
chatter.publish(&str_msg);
// Handle all communications and callbacks.
nh.spinOnce();
// Delay for a bit.
nh.getHardware()->delay(100);
}
}
void setup() {
hardware.init();
node_handle.subscribe(subscriber);
node_handle.advertise(publisher);
}
/// <summary>
/// Runs the application
/// </summary>
/// <param name="hInstance">handle to the application instance</param>
/// <param name="nCmdShow">whether to display minimized, maximized, or normally</param>
/// <returns>WPARAM of final message as int</returns>
int CMainWindow::Run(HINSTANCE hInstance, int nCmdShow)
{
sprintf_s(rosSrvrIp, "192.168.1.134");
nh.initNode(rosSrvrIp, port);
nh.advertise(sweep_pub);
// Create application window
if (FAILED(CreateMainWindow(hInstance)))
{
return 0;
}
// Create mutexes
m_hColorResolutionMutex = CreateMutex(NULL, FALSE, NULL);
m_hDepthResolutionMutex = CreateMutex(NULL, FALSE, NULL);
m_hColorBitmapMutex = CreateMutex(NULL, FALSE, NULL);
m_hDepthBitmapMutex = CreateMutex(NULL, FALSE, NULL);
m_hPaintWindowMutex = CreateMutex(NULL, FALSE, NULL);
// Initialize default menu options and resolutions
InitSettings(GetMenu(m_hWndMain));
// Create bitmaps and fonts
CreateStreamInformationFont();
CreateColorImage();
CreateDepthImage();
CreateDepthImagePrev();
// Perform Kinect initialization
// If Kinect initialization succeeded, start the event processing thread
// that will update the screen with depth and color images
if (SUCCEEDED(CreateFirstConnected()))
{
// Create window processing thread
m_hProcessStopEvent = CreateEvent(NULL, FALSE, FALSE, NULL);
m_hProcessThread = CreateThread(NULL, 0, ProcessThread, this, 0, NULL);
NuiSetDeviceStatusCallback( &CMainWindow::StatusProc, this );
}
// If Kinect initialization failed, disable the menus
else
{
DisableMenus();
}
// Display window
ResizeWindow();
ShowWindow(m_hWndMain, nCmdShow);
// Main message loop:
MSG msg;
while (GetMessage(&msg, NULL, 0, 0))
{
// Process message
TranslateMessage(&msg);
DispatchMessage(&msg);
nh.spinOnce();
}
return static_cast<int>(msg.wParam);
}
int main(int argc, char* argv[])
{
// These following lines are used to Make sure that command lline args are correct
if(argc<5) // if number of args are less than 5
{
cerr << "Usage: " << argv[0] << " <socket> <left_motor_port> <right_motor_port> <sensor_port> <hz>" << endl;
return 1;
}
int milliseconds = 100;
if(argc==6)
milliseconds = 1000/atoi(argv[5]);
// cout<<"milliseconds"<<milliseconds;
string left_motor_port(argv[2]);
string right_motor_port(argv[3]);
string sensor_port(argv[4]);
// if(left_motor_port<1||left_motor_port>4||right_motor_port<1||right_motor_port>4||left_motor_port==right_motor_port)
// {
// cerr << "Invalid motor port numbers. Must be 1, 2, 3 or 4 and distinct." << endl;
// return 1;
// }
// TODO: Check if both are of same type
left_motor = motor(left_motor_port);
right_motor = motor(right_motor_port);
s = sensor(sensor_port);
if(s.type()!="ev3-uart-30") // sensor object s will not be used hereafter
{
cerr << "Invalid sensor type. Must be EV3 ultrasonic. Given sensor is of type " << s.type() << endl;
return 1;
}
//---------------------------------------------------------------------------------------------------------------------------------------------
// TODO: Check if both were initialised
left_motor.reset();
left_motor.set_position(0);
left_motor.set_run_mode("time");// changed from forever mode to time
left_motor.set_stop_mode("brake");
left_motor.set_regulation_mode("on");
right_motor.reset();
right_motor.set_position(0);
right_motor.set_run_mode("time");
right_motor.set_stop_mode("brake");
right_motor.set_regulation_mode("on");
nav_msgs::Odometry odom_msg;// msg object for the publisher
ros::Publisher odom_pub("/robot3/odom"/*topic name*/, &odom_msg);
nh.advertise(odom_pub); // advertises that odom_pub is the publisher name
tf::TransformBroadcaster odom_broadcaster; //broadcaster msg for the odometry
tf::TransformBroadcaster scan_broadcaster; // broadcaster for the ultrasonic sensor
nh.subscribe(pose_sub);
ros::Time current_time, last_time;
current_time = nh.now();
last_time = nh.now();
nh.initNode(argv[1]);
odom_broadcaster.init(nh);
//nh.advertiseService(server1);
nh.advertiseService(server2);
while(!nh.connected()) {nh.spinOnce();}
// JUST TO KNOW IF THE NODE IS ALIVE
cout<<"1. Gostraight service\n2.Trace an arc service"<<endl;
while(1) {
// check for incoming messages
current_time = nh.now();
geometry_msgs::Quaternion odom_quat = tf::createQuaternionFromYaw(t); // odom_quat stores Quaternion cretaed from yaw
// cout<<" Calculated quat: "<<endl;
//first, we'll publish the transform over tf
geometry_msgs::TransformStamped odom_trans;//message object
odom_trans.header.stamp = current_time;
odom_trans.header.frame_id = map_name;//map_name is just given as map
const char base_link_name[18] = "/robot3/base_link";
odom_trans.child_frame_id = base_link_name;
// cout<<" constructed header"<<endl;
// loading the message object with data
odom_trans.transform.translation.x = x;
odom_trans.transform.translation.y = y;
odom_trans.transform.translation.z = 0.0;// as we are dealing with xy plane
odom_trans.transform.rotation = odom_quat;// this is quaternion created from yaw angle t
// cout<<" Constructed full message"<<endl;
//send the transform
odom_broadcaster.sendTransform(odom_trans);
//next, we'll publish the odometry message over ROS
odom_msg.header.stamp = current_time;
odom_msg.header.frame_id = map_name;
//set the position
odom_msg.pose.pose.position.x = x;
odom_msg.pose.pose.position.y = y;
odom_msg.pose.pose.position.z = 0.0;
odom_msg.pose.pose.orientation = odom_quat;
//set the velocity
odom_msg.child_frame_id = base_link_name;
odom_msg.twist.twist.linear.x = vx;
odom_msg.twist.twist.linear.y = 0;
odom_msg.twist.twist.angular.z = wt;
// cout<<" sizeof "<<sizeof(odom_msg)<<endl;
//publish the message
odom_pub.publish(&odom_msg);
cout<<"Services are being advertised. Waiting for request"<<endl;
nh.spinOnce();
sleep(1);
}
return 0;
}
void setup() {
hardware.init();
nh.advertise(p);
}