int main(int argc, char **argv)
{
ros::init(argc, argv, NODE_NAME);
std::string language = "en_US";
std::string grammar = "walking";
geometry_msgs::TwistPtr cmd_vel (new geometry_msgs::Twist());
cmd_vel->linear.x = 0;
cmd_vel->linear.y = 0;
cmd_vel->angular.z = 0;
/* Create a ros node */
ros::NodeHandle nh(NODE_NAME);
ROS_INFO_STREAM("Creating node " << NODE_NAME);
/* create a service client and store it in client */
ros::ServiceClient client = nh.serviceClient<pal_interaction_msgs::recognizerService>(ASR_SERVICE);
/* create a subscribed and store it in subscriber */
ros::Subscriber subscriber = nh.subscribe<pal_interaction_msgs::asrresult>(ASR_TOPIC, 0, boost::bind(asrResultsCallback,_1, cmd_vel));
/* construct a service message to send to the asrservice
* in order to activate the ASR with walking grammar in the English language */
pal_interaction_msgs::recognizerService srv;
srv.request.asrupdate.language = language;
srv.request.asrupdate.enable_grammar = grammar;
srv.request.asrupdate.active = true;
/* call the service */
if (client.call(srv))
{
ROS_INFO("ASR enabled");
}
else
{
ROS_ERROR_STREAM("Failed to call service " << ASR_SERVICE);
exit(1);
}
sleep(5);
/* Say some instructions to the user */
say("Now, you can command me with voice commands. You can say \"step forward\", \"step backward\", \"move right\", \"move left\", \"turn right\" or \"turn left\".", "en_GB");
boost::thread p(boost::bind(myRosSpin, cmd_vel));
while(ros::ok())
{
ros::spinOnce();
sleep(0.5);
}
return 0;
}
void DiffDrivePoseControllerROS::setControlOutput()
{
geometry_msgs::TwistPtr cmd_vel(new geometry_msgs::Twist());
if (!pose_reached_)
{
cmd_vel->linear.x = v_;
cmd_vel->angular.z = w_;
}
command_velocity_publisher_.publish(cmd_vel);
}
/*!
* \brief Executes the callback from the command_vel topic.
*
* Set the current velocity target.
* \param msg JointVelocities
*/
void topicCallback_CommandVel(const brics_actuator::JointVelocities::ConstPtr& msg)
{
ROS_DEBUG("Received new velocity command");
if (!initialized_)
{
ROS_WARN("Skipping command: powercubes not initialized");
publishState(false);
return;
}
if (pc_ctrl_->getPC_Status() != PowerCubeCtrl::PC_CTRL_OK)
{
publishState(false);
return;
}
PowerCubeCtrl::PC_CTRL_STATUS status;
std::vector<std::string> errorMessages;
pc_ctrl_->getStatus(status, errorMessages);
/// ToDo: don't rely on position of joint names, but merge them (check between msg.joint_uri and member variable JointStates)
unsigned int DOF = pc_params_->GetDOF();
std::vector<std::string> jointNames = pc_params_->GetJointNames();
std::vector<double> cmd_vel(DOF);
std::string unit = "rad";
/// check dimensions
if (msg->velocities.size() != DOF)
{
ROS_ERROR("Skipping command: Commanded velocities and DOF are not same dimension.");
return;
}
/// parse velocities
for (unsigned int i = 0; i < DOF; i++)
{
/// check joint name
if (msg->velocities[i].joint_uri != jointNames[i])
{
ROS_ERROR("Skipping command: Received joint name %s doesn't match expected joint name %s for joint %d.",msg->velocities[i].joint_uri.c_str(),jointNames[i].c_str(),i);
return;
}
/// check unit
if (msg->velocities[i].unit != unit)
{
ROS_ERROR("Skipping command: Received unit %s doesn't match expected unit %s.",msg->velocities[i].unit.c_str(),unit.c_str());
return;
}
/// if all checks are successful, parse the velocity value for this joint
ROS_DEBUG("Parsing velocity %f for joint %s",msg->velocities[i].value,jointNames[i].c_str());
cmd_vel[i] = msg->velocities[i].value;
}
/// command velocities to powercubes
if (!pc_ctrl_->MoveVel(cmd_vel))
{
error_ = true;
error_msg_ = pc_ctrl_->getErrorMessage();
ROS_ERROR("Skipping command: %s",pc_ctrl_->getErrorMessage().c_str());
return;
}
ROS_DEBUG("Executed velocity command");
publishState(false);
}
