bool getJointVelocityLimits(const std::string urdf_param_name, std::map<std::string, double> &velocity_limits)
{
urdf::Model model;
std::map<std::string, boost::shared_ptr<urdf::Joint> >::iterator iter;
if (!ros::param::has(urdf_param_name) || !model.initParam(urdf_param_name))
return false;
velocity_limits.clear();
for (iter=model.joints_.begin(); iter!=model.joints_.end(); ++iter)
{
std::string joint_name(iter->first);
boost::shared_ptr<urdf::JointLimits> limits = iter->second->limits;
if ( limits && (limits->velocity > 0) )
velocity_limits.insert(std::pair<std::string,double>(joint_name,limits->velocity));
}
return true;
}
void joint_state_CB(const sensor_msgs::JointState& joint_states) {
double dtheta_right, dtheta_left, ds, dpsi;
int n_joints = joint_states.name.size();
int ijnt;
int njnts_found = 0;
bool found_name = false;
double R_LEFT_WHEEL = R_WHEEL*LEFT_OVER_RIGHT;
double R_RIGHT_WHEEL = R_WHEEL/LEFT_OVER_RIGHT;
g_old_left_wheel_ang = g_new_left_wheel_ang;
g_old_right_wheel_ang = g_new_right_wheel_ang;
g_t_old = g_t_new;
g_cur_time = ros::Time::now();
g_t_new = g_cur_time.toSec();
g_dt = g_t_new - g_t_old;
for (ijnt = 0; ijnt < n_joints; ijnt++) {
std::string joint_name(joint_states.name[ijnt]);
if (joint_name.compare("ch1") == 0) {
g_new_left_wheel_ang = joint_states.position[ijnt];
njnts_found++;
}
if (joint_name.compare("ch2") == 0) {
g_new_right_wheel_ang = joint_states.position[ijnt];
njnts_found++;
}
}
if (njnts_found < 2) {
//ROS_WARN("did not find both wheel joint angles!");
//for (ijnt = 0; ijnt < n_joints; ijnt++) {
// std::cout<<joint_states.name[ijnt]<<std::endl;
//}
}
else {
//ROS_INFO("found both wheel joint names");
}
if (!joints_states_good) {
if (g_new_left_wheel_ang > wheel_ang_sham_init / 2.0) {
joints_states_good = true; //passed the test
g_old_left_wheel_ang = g_new_left_wheel_ang;
g_old_right_wheel_ang = g_new_right_wheel_ang; //assume right is good now as well
}
}
if (joints_states_good) { //only compute odom if wheel angles are valid
dtheta_left = g_new_left_wheel_ang - g_old_left_wheel_ang;
dtheta_right = g_new_right_wheel_ang - g_old_right_wheel_ang;
ds = 0.5 * (dtheta_left * R_LEFT_WHEEL + dtheta_right * R_RIGHT_WHEEL);
dpsi = dtheta_right * R_RIGHT_WHEEL / TRACK - dtheta_left * R_LEFT_WHEEL / TRACK;
g_drifty_odom.pose.pose.position.x += ds * cos(g_odom_psi);
g_drifty_odom.pose.pose.position.y += ds * sin(g_odom_psi);
g_odom_psi += dpsi;
//ROS_INFO("dthetal, dthetar, dpsi, odom_psi, dx, dy= %f, %f %f, %f %f %f", dtheta_left, dtheta_right, dpsi, g_odom_psi,
// ds * cos(g_odom_psi), ds * sin(g_odom_psi));
g_drifty_odom.pose.pose.orientation = convertPlanarPsi2Quaternion(g_odom_psi);
g_drifty_odom.twist.twist.linear.x = ds / g_dt;
g_drifty_odom.twist.twist.angular.z = dpsi / g_dt;
g_drifty_odom.header.stamp = g_cur_time;
g_drifty_odom_pub.publish(g_drifty_odom);
// ROS_INFO("%f",g_odom_psi/3.14159);
}
}
boost::shared_ptr<GraspSpecification > GraspSpecification::readFromUrdf(const std::string &filename) {
TiXmlDocument doc( filename );
doc.LoadFile();
std::unique_ptr<GraspSpecification > u_gs(new GraspSpecification);
if (doc.Error())
{
std::cout << "ERROR: GraspSpecification::readFromXml: " << doc.ErrorDesc() << std::endl;
doc.ClearError();
return boost::shared_ptr<GraspSpecification >(NULL);
}
TiXmlElement *elementR = doc.FirstChildElement("robot");
if (!elementR)
{
std::cout << "ERROR: GraspSpecification::readFromXml: " << "Could not find the 'robot' element in the xml file" << std::endl;
return boost::shared_ptr<GraspSpecification >(NULL);
}
int grasp_specs_count = 0;
for (TiXmlElement* elementGS = elementR->FirstChildElement("grasp_specification"); elementGS; elementGS = elementGS->NextSiblingElement("grasp_specification")) {
grasp_specs_count++;
}
if (grasp_specs_count != 1) {
std::cout << "ERROR: GraspSpecification::readFromXml: wrong number of grasp_specification elements: " << grasp_specs_count << std::endl;
return boost::shared_ptr<GraspSpecification >(NULL);
}
TiXmlElement* elementGS = elementR->FirstChildElement("grasp_specification");
for (TiXmlElement* elementI = elementGS->FirstChildElement("init_state"); elementI; elementI = elementI->NextSiblingElement("init_state")) {
const char *str = elementI->Attribute("joint");
if (!str) {
std::cout << "ERROR: GraspSpecification::readFromXml: init_state joint" << std::endl;
return boost::shared_ptr<GraspSpecification >(NULL);
}
std::string joint_name( str );
str = elementI->Attribute("position");
if (!str) {
std::cout << "ERROR: GraspSpecification::readFromXml: init_state position" << std::endl;
return boost::shared_ptr<GraspSpecification >(NULL);
}
u_gs->q_init_map_[joint_name] = string2double( str );
}
for (TiXmlElement* elementG = elementGS->FirstChildElement("goal_state"); elementG; elementG = elementG->NextSiblingElement("goal_state")) {
const char *str = elementG->Attribute("joint");
if (!str) {
std::cout << "ERROR: GraspSpecification::readFromXml: init_state joint" << std::endl;
return boost::shared_ptr<GraspSpecification >(NULL);
}
std::string joint_name( str );
str = elementG->Attribute("position");
if (!str) {
std::cout << "ERROR: GraspSpecification::readFromXml: init_state position" << std::endl;
return boost::shared_ptr<GraspSpecification >(NULL);
}
u_gs->q_goal_map_[joint_name] = string2double( str );
}
return boost::shared_ptr<GraspSpecification >(new GraspSpecification(*u_gs.get()));
}
bool ReferenceGenerator::setGoal(const control_msgs::FollowJointTrajectoryGoal& goal_msg, std::string& id, std::stringstream& ss)
{
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
if (id.empty())
{
std::stringstream s;
s << "goal-" << (next_goal_id_++);
id = s.str();
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
if (goals_.find(id) != goals_.end())
{
ss << "Goal with id '" << id << " already exists.\n";
return false;
}
JointGoal& goal = goals_[id];
goal.goal_msg = goal_msg;
goal.num_goal_joints = goal_msg.trajectory.joint_names.size();
goal.joint_index_mapping.resize(goal.num_goal_joints);
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
// Check feasibility of joint goals
std::set<std::string> goals_to_cancel;
bool goal_ok = true;
for (unsigned int i = 0; i < goal.num_goal_joints; ++i)
{
const std::string& joint_name = goal_msg.trajectory.joint_names[i];
int idx = joint_index(joint_name);
if (idx < 0)
{
ss << "Unknown joint: '" << joint_name << "'.\n";
goal_ok = false;
continue;
}
const JointInfo& js = joint_info_[idx];
if (!js.goal_id.empty())
goals_to_cancel.insert(js.goal_id);
if (!js.is_set)
{
ss << "Joint '" << joint_name << "' initial position and velocity is not set.\n";
goal_ok = false;
}
if (js.max_vel == 0 || js.max_acc == 0 || (js.min_pos == js.max_pos))
{
ss << "Joint '" << joint_name << "' limits not initialized: "
<< "max vel = " << js.max_vel << ", max acc = " << js.max_acc
<< ", min pos = " << js.min_pos << ", max pos = " << js.max_pos << "\n";
goal_ok = false;
}
goal.joint_index_mapping[i] = idx;
}
if (!goal_ok)
{
goals_.erase(id);
return false;
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
// Check if joint goals go out of limits
for (unsigned int i = 0; i < goal_msg.trajectory.points.size(); ++i)
{
const trajectory_msgs::JointTrajectoryPoint& p = goal_msg.trajectory.points[i];
for(unsigned int j = 0; j < goal.num_goal_joints; ++j)
{
unsigned int joint_idx = goal.joint_index_mapping[j];
const JointInfo& js = joint_info_[joint_idx];
double pos = p.positions[j];
if (pos < js.min_pos || pos > js.max_pos)
{
ss << "Joint '" << joint_name(joint_idx) << "' goes out of limits in point " << i << " "
<< "(min = " << js.min_pos << ", max = " << js.max_pos << ", requested goal = " << pos << ").\n";
goal_ok = false;
}
}
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
if (!goal_ok)
{
goals_.erase(id);
return false;
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
// Cancel overlapping goals
for(std::set<std::string>::const_iterator it = goals_to_cancel.begin(); it != goals_to_cancel.end(); ++it)
cancelGoal(*it);
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
for(unsigned int i = 0; i < goal.num_goal_joints; ++i)
joint_info_[goal.joint_index_mapping[i]].goal_id = id;
goal.sub_goal_idx = -1;
goal.time_since_start = 0;
goal.use_cubic_interpolation = false;
return true;
}
