ExpressionDriver(std::string name, ros::NodeHandle &n) : nh_(n), nPriv("~"), as_(nh_, name, boost::bind(&ExpressionDriver::executeCB, this, _1), false),
action_name_()
{
//Initialize the port
nPriv.param<std::string>("port", port, "/dev/ttyACM0");
nPriv.param<int>("baudrate", baudrate, 115200);
try
{
driverComms = new DriverComms(port, baudrate);
}
catch(const std::exception&)
{
throw std::exception();
}
//Start the action server
as_.start();
//Initialize and set the default expression
bool sentM, sentS, sentR, sentL;
sentM = driverComms->sendCommand(ExpressionValues::ExpressionHappy::MOUTH, vizzy_expressions::ExpressionActionGoal::_goal_type::PART_MOUTH);
feedback_.mouth_value = ExpressionValues::ExpressionHappy::MOUTH;
feedback_.mouth_emotion = vizzy_expressions::ExpressionActionGoal::_goal_type::FACE_HAPPY;
sentS = driverComms->sendCommand(ExpressionValues::ExpressionHappy::EYELIDS, vizzy_expressions::ExpressionActionGoal::_goal_type::PART_EYELIDS);
feedback_.eyelids_emotion = vizzy_expressions::ExpressionActionGoal::_goal_type::FACE_HAPPY;
feedback_.eyelids_value = ExpressionValues::ExpressionHappy::EYELIDS;
sentR = driverComms->sendCommand(ExpressionValues::ExpressionHappy::RIGHT_EYEBROW, vizzy_expressions::ExpressionActionGoal::_goal_type::PART_RIGHTEYEBROW);
feedback_.rightEyebrow_emotion = vizzy_expressions::ExpressionActionGoal::_goal_type::FACE_HAPPY;
feedback_.rightEyebrow_value = ExpressionValues::ExpressionHappy::RIGHT_EYEBROW;
sentL = driverComms->sendCommand(ExpressionValues::ExpressionHappy::LEFT_EYEBROW, vizzy_expressions::ExpressionActionGoal::_goal_type::PART_LEFTEYEBROW);
feedback_.leftEyebrow_emotion = vizzy_expressions::ExpressionActionGoal::_goal_type::FACE_HAPPY;
feedback_.leftEyebrow_value = ExpressionValues::ExpressionHappy::LEFT_EYEBROW;
if(sentM && sentS && sentR && sentL)
{
ROS_INFO("Initialization commands sent. Vizzy is Happy! :D");
}
else
{
ROS_ERROR("Expression initialization failed. Check the board? :(");
as_.shutdown();
delete driverComms;
throw std::exception();
}
}
