~fuzzgun/buttonui-ros-pkg/trunk : revision 20

58

};

59

60

enum {

61

SOUND_ENABLED=0,

62

SOUND_GOAL_ARRIVED,

63

SOUND_GOAL_FAILED,

64

SOUND_ESTOP_ACTIVE,

65

SOUND_ESTOP_PRESSED,

66

SOUND_ESTOP_RELEASED,

67

GENERAL_SOUNDS

61

EVENT_ENABLED=0,

62

EVENT_GOAL_ARRIVED,

63

EVENT_GOAL_FAILED,

64

EVENT_ESTOP_ACTIVE,

65

EVENT_ESTOP_PRESSED,

66

EVENT_ESTOP_RELEASED,

67

GENERAL_EVENTS

68

};

69

70

std::string general_sound[] = {

70

std::string general_event[] = {

71

"User interface enabled",

72

"Arrived at %s",

73

"I can't get to %s",

76

"Emergency stop released"

77

};

78

79

// commandline instructions when general events occur

80

std::string general_command[] = {

81

"","","","","",""

82

};

79

83

80

84

// Topic subscribed to in order to obtain poses, typically /acml_pose

81

85

std::string pose_topic = "";

144

148

}

145

149

}

146

150

151

// runs a shell command when a general event occurs

152

void run_general_command(int command)

153

{

154

if( (command < 0) || (command >= GENERAL_EVENTS)) return;

155

156

if (general_command[command] != "") {

157

if (system(general_command[command].c_str())==0) {

158

ROS_INFO("Running command:\n%s",general_command[command].c_str());

159

}

160

}

161

}

162

163

// runs a shell command when a button is pressed

164

void run_button_command(int command)

165

{

166

if (button_command[command] != "") {

167

if (system(button_command[command].c_str())==0) {

168

ROS_INFO("Running command:\n%s",button_command[command].c_str());

169

}

170

}

171

}

172

147

173

/*!

148

174

* \brief loads place names from a comma separated string. eg. Kitchen table,Lounge,Hallway

149

175

* \param placenames string containing place names

172

198

}

173

199

}

174

200

201

// If currently moving towards a goal location then abandon the navigation goal

202

void abandon_goals()

203

{

204

if (ac!=NULL) {

205

ac->cancelAllGoals();

206

moving_to_goal=false;

207

}

208

}

209

175

210

// Receive button presses from the Arduino

176

211

int get_button_press(

177

212

SerialPort &serial,

185

220

char command = (char)comBuffer[0];

186

221

if ((command>='0') && (command<='9')) {

187

222

if (!Estop) {

223

// A button was pressed

188

224

index = atoi((char*)comBuffer);

189

225

int index2 = 0;

190

226

for (index2=0;index2<(int)(sizeof(button_index)/sizeof(int));index2++) {

194

230

index = index2;

195

231

}

196

232

else {

197

say(general_sound[SOUND_ESTOP_ACTIVE], speak_command);

233

// A button was pressed but the E-stop is active

234

run_general_command(EVENT_ESTOP_ACTIVE);

235

say(general_event[EVENT_ESTOP_ACTIVE], speak_command);

198

236

ROS_WARN("Button press prevented by E-stop");

199

237

}

200

238

}

202

240

// E-stop released

203

241

if ((Estop) && (command=='e')) {

204

242

Estop = false;

205

say(general_sound[SOUND_ESTOP_RELEASED], speak_command);

243

run_general_command(EVENT_ESTOP_RELEASED);

244

say(general_event[EVENT_ESTOP_RELEASED], speak_command);

206

245

ROS_WARN("E-stop released");

207

246

}

208

247

209

248

// E-stop activated

210

249

if ((!Estop) && (command=='E')) {

250

abandon_goals();

251

stop_motors(initialised, cmd);

252

started=false;

211

253

Estop = true;

212

say(general_sound[SOUND_ESTOP_PRESSED], speak_command);

254

run_general_command(EVENT_ESTOP_PRESSED);

255

say(general_event[EVENT_ESTOP_PRESSED], speak_command);

213

256

ROS_WARN("E-stop activated");

214

257

}

215

258

}

217

260

return index;

218

261

}

219

262

263

// make an announcement when a button is pressed

264

void button_announce(int button)

265

{

266

if (button_sound[button] !="") say(button_sound[button],speak_command);

267

}

268

220

269

// Do stuff when buttons are pressed

221

270

void update(bool * prev_enable, bool * prev_started, int index)

222

271

{

244

293

started = true;

245

294

246

295

// load the initial pose

247

if (button_sound[BUTTON_START] !="") say(button_sound[BUTTON_START],speak_command);

248

if (button_command[BUTTON_START] != "") {

249

if (system(button_command[BUTTON_START].c_str())==0) {

250

ROS_INFO("Running command:\n%s",button_command[BUTTON_START].c_str());

251

}

252

}

253

296

button_announce(BUTTON_START);

297

run_button_command(BUTTON_START);

254

298

ROS_INFO("Start button pressed");

255

299

}

256

300

break;

258

302

case BUTTON_STOP: {

259

303

if (started) {

260

304

started = false;

261

if (ac!=NULL) {

262

ac->cancelAllGoals();

263

moving_to_goal=false;

264

}

265

266

if (button_sound[BUTTON_STOP] !="") say(button_sound[BUTTON_STOP],speak_command);

267

if (button_command[BUTTON_STOP] != "") {

268

if (system(button_command[BUTTON_STOP].c_str())==0) {

269

ROS_INFO("Running command:\n%s",button_command[BUTTON_STOP].c_str());

270

}

271

}

272

305

abandon_goals();

306

button_announce(BUTTON_STOP);

307

run_button_command(BUTTON_STOP);

273

308

ROS_INFO("Stop button pressed");

274

309

}

275

310

break;

338

373

}

339

374

340

375

if ((en) && (!*prev_enable)) {

341

if (general_sound[SOUND_ENABLED] !="") say(general_sound[SOUND_ENABLED],speak_command);

376

if (general_event[EVENT_ENABLED] !="") say(general_event[EVENT_ENABLED],speak_command);

342

377

}

343

378

344

379

if (moving_to_goal) {

345

380

if (ac->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) {

346

381

char saystr[255];

347

sprintf((char*)saystr,general_sound[SOUND_GOAL_ARRIVED].c_str(),place_names[current_place_name_index].c_str());

382

run_general_command(EVENT_GOAL_ARRIVED);

383

sprintf((char*)saystr,general_event[EVENT_GOAL_ARRIVED].c_str(),place_names[current_place_name_index].c_str());

348

384

ROS_INFO("%s",saystr);

349

385

say(saystr,speak_command);

350

386

// Proceed to the next location

356

392

(ac->getState() == actionlib::SimpleClientGoalState::REJECTED) ||

357

393

(ac->getState() == actionlib::SimpleClientGoalState::ABORTED)) {

358

394

char saystr[255];

359

sprintf((char*)saystr,general_sound[SOUND_GOAL_FAILED].c_str(),place_names[current_place_name_index].c_str());

395

run_general_command(EVENT_GOAL_FAILED);

396

sprintf((char*)saystr,general_event[EVENT_GOAL_FAILED].c_str(),place_names[current_place_name_index].c_str());

360

397

ROS_INFO("%s",saystr);

361

398

say(saystr,speak_command);

362

399

moving_to_goal = false;

408

445

if (sound_str != "") button_sound[BUTTON_GO] = sound_str;

409

446

sound_str = "";

410

447

nh.getParam("enablesound", sound_str);

411

if (sound_str != "") general_sound[SOUND_ENABLED] = sound_str;

448

if (sound_str != "") general_event[EVENT_ENABLED] = sound_str;

412

449

sound_str = "";

413

450

nh.getParam("estopactivesound", sound_str);

414

if (sound_str != "") general_sound[SOUND_ESTOP_ACTIVE] = sound_str;

451

if (sound_str != "") general_event[EVENT_ESTOP_ACTIVE] = sound_str;

415

452

sound_str = "";

416

453

nh.getParam("estoppressedsound", sound_str);

417

if (sound_str != "") general_sound[SOUND_ESTOP_PRESSED] = sound_str;

454

if (sound_str != "") general_event[EVENT_ESTOP_PRESSED] = sound_str;

418

455

sound_str = "";

419

456

nh.getParam("estopreleasedsound", sound_str);

420

if (sound_str != "") general_sound[SOUND_ESTOP_RELEASED] = sound_str;

457

if (sound_str != "") general_event[EVENT_ESTOP_RELEASED] = sound_str;

421

458

422

459

// start and stop button digital inputs on the interface kit

423

460

int i=-1;

461

nh.getParam("startbuttonindex", i);

462

if (i>-1) button_index[BUTTON_START] = i;

463

i=-1;

464

nh.getParam("stopbuttonindex", i);

465

if (i>-1) button_index[BUTTON_STOP] = i;

466

467

// commands

424

468

std::string str="";

425

nh.getParam("startbuttonindex", i);

426

if (i>-1) button_index[BUTTON_START] = i;

427

469

nh.getParam("startbuttoncommand", str);

428

470

if (str!="") button_command[BUTTON_START] = str;

429

i=-1;

430

nh.getParam("stopbuttonindex", i);

431

if (i>-1) button_index[BUTTON_STOP] = i;

432

471

str="";

433

472

nh.getParam("stopbuttoncommand", str);

434

473

if (str!="") button_command[BUTTON_STOP] = str;

474

str="";

475

nh.getParam("estoppressedcommand", str);

476

if (str!="") general_command[EVENT_ESTOP_PRESSED] = str;

477

str="";

478

nh.getParam("estopreleasedcommand", str);

479

if (str!="") general_command[EVENT_ESTOP_RELEASED] = str;

480

str="";

481

nh.getParam("estopactivecommand", str);

482

if (str!="") general_command[EVENT_ESTOP_ACTIVE] = str;

483

str="";

484

nh.getParam("goalarrivedcommand", str);

485

if (str!="") general_command[EVENT_GOAL_ARRIVED] = str;

486

str="";

487

nh.getParam("goalfailedcommand", str);

488

if (str!="") general_command[EVENT_GOAL_FAILED] = str;

435

489

436

490

// Button for learning new locations/poses

437

491

std::string placenames = "";