void MentalPerspectiveTransformer::callback(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& cloud_in) {
static tf::TransformListener listener;
const std::string source_frame = "robot_head";
const std::string target_frame = "head_origin1";
// first get cloud in the source frame (it is usually in camera frame)
pcl::PointCloud<pcl::PointXYZRGB> cloud_in_source_frame;
pcl_ros::transformPointCloud(source_frame, *cloud_in, cloud_in_source_frame, listener);
static tf::TransformBroadcaster br;
try {
// get transform from source to target frame
tf::StampedTransform transform_source_target;
listener.waitForTransform(target_frame, source_frame, pcl_conversions::fromPCL(cloud_in_source_frame.header.stamp), ros::Duration(1.0));
listener.lookupTransform (target_frame, source_frame, ros::Time(0), transform_source_target);
ROS_INFO_STREAM("Origin: " << transform_source_target.getOrigin().getX() << " "
<< transform_source_target.getOrigin().getY() << " "
<< transform_source_target.getOrigin().getZ());
ROS_INFO_STREAM("Angle: " << transform_source_target.getRotation().getAngle());
ROS_INFO_STREAM("Axis: " << transform_source_target.getRotation().getAxis().getX() << " "
<< transform_source_target.getRotation().getAxis().getY() << " "
<< transform_source_target.getRotation().getAxis().getZ());
tf::Quaternion q_origin = tf::Quaternion::getIdentity();
tf::Quaternion q_dest = transform_source_target.getRotation();
tf::Vector3 v_dest = transform_source_target.getOrigin();
//ROS_INFO_STREAM("q_dest: " << q_dest.getX() << " " << q_dest.getY() << " " << q_dest.getZ() << " " << q_dest.getW());
/*tf::Vector3 v_r = v_dest/2.0;
ROS_INFO_STREAM("v_r: " << v_r.getX() << " " << v_r.getY() << " " << v_r.getZ());
tf::Vector3 c = tf::Vector3(-1.0*v_r[1], v_r[0], 0);
tf::Vector3 p_circle = v_r + c;
ROS_INFO_STREAM("p_circle " << p_circle.getX() << " " << p_circle.getY() << " " << p_circle.getZ());
double radius = v_r.length();
ROS_INFO_STREAM("radius " << radius);
tf::Vector3 n = p_circle.cross(v_r).normalize();
if(transform.getRotation().getAngle()>M_PI) {
n = -1.0*n;
}
ROS_INFO_STREAM("n " << n.getX() << " " << n.getY() << " " << n.getZ());
tf::Vector3 u = v_dest.normalized();
ROS_INFO_STREAM("u " << u.getX() << " " << u.getY() << " " << u.getZ());*/
// compute circle equation
double r1x = (pow(v_dest.getX(), 2.0) + pow(v_dest.getY(), 2.0) + pow(v_dest.getZ(), 2.0)) / (2.0*(v_dest.getX()+pow(v_dest.getZ(), 2.0)/v_dest.getX()));
double r1y = 0.0;
double r1z = r1x * v_dest.getZ()/v_dest.getX();
tf::Vector3 r1(r1x, r1y, r1z);
tf::Vector3 r2(v_dest.getX() - r1x, v_dest.getY() - r1y, v_dest.getZ() - r1z);
double radius = r1.length();
tf::Vector3 n = v_dest.cross(2*r1).normalize();
tf::Vector3 u = r1.normalized();
tf::Vector3 v_r = r1;
ROS_INFO_STREAM("u " << u.getX() << " " << u.getY() << " " << u.getZ());
ROS_INFO_STREAM("n " << n.getX() << " " << n.getY() << " " << n.getZ());
ROS_INFO_STREAM("r1 " << r1x << " " << r1y << " " << r1z);
ROS_INFO_STREAM("r2 " << r2.getX() << " " << r2.getY() << " " << r2.getZ());
ROS_INFO_STREAM("radius " << radius);
// get theta for head coordinates
perspective_taking_python::CircleEquation ce;
ce.request.radius = radius;
tf::vector3TFToMsg(v_dest, ce.request.h);
tf::vector3TFToMsg(n, ce.request.n);
tf::vector3TFToMsg(u, ce.request.u);
tf::vector3TFToMsg(v_r, ce.request.c);
if(equationSolverClient_.call(ce)) {
ROS_INFO_STREAM("response: " << ce.response.theta);
} else {
ROS_ERROR("Did not get response from equationSolverClient!");
return;
}
double delta = 0.01; // approximately 10 steps at 1.3 meters distance to human (straight ahead)
double epsilon = M_PI * delta / radius;
double resp_theta = ce.response.theta;
ROS_INFO_STREAM("epsilon: " << epsilon);
for(double theta = -1.0, step=0; theta<=resp_theta; theta+=epsilon, step+=1) {
double b = 1.0/(resp_theta+1.0);
if(transform_source_target.getRotation().getAngle()>M_PI) {
b = -1.0*b;
}
double theta_for_q = b * theta + b;
tf::Quaternion q_intermediate = q_origin.slerp(q_dest, theta_for_q);
//tf::Quaternion q_intermediate = q_origin.slerp(q_dest, 1.0-theta);
//tf::Vector3 v_intermediate = v_origin.lerp(v_dest, ration);
tf::Vector3 v_intermediate = radius*cos(M_PI*theta)*u + radius*sin(M_PI*theta)*n.cross(u)+v_r;
ROS_INFO_STREAM("theta_for_q: " << theta_for_q);
ROS_INFO_STREAM("theta: " << theta << ": " << v_intermediate.getX() << " " << v_intermediate.getY() << " " << v_intermediate.getZ());
tf::Transform intermediate_transform;
intermediate_transform.setOrigin(v_intermediate);
intermediate_transform.setRotation(q_intermediate);
tf::Transform intermediate_transform_inv = intermediate_transform.inverse();
std::stringstream ss; ss << std::fixed << setprecision(2) << step;
br.sendTransform(tf::StampedTransform(intermediate_transform_inv, ros::Time::now(), source_frame, "rot"+ss.str()));
ROS_INFO_STREAM("step: " << step);
}
pcl::PointCloud<pcl::PointXYZRGB> output;
pcl_ros::transformPointCloud(target_frame, cloud_in_source_frame, output, listener);
transformedCloudPublisher_.publish(output);
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
}
}
bool WifiStumbler::stumble()
{
int pid;
pid = getpid();
struct iwreq w_request;
w_request.u.data.pointer = (caddr_t)&pid;
w_request.u.data.length = 0;
if (iw_set_ext(wlan_sock_, wlan_if_.c_str(), SIOCSIWSCAN, &w_request) < 0)
{
ROS_ERROR("Couldn't start stumbler.");
return false;
}
timeval time;
time.tv_sec = 0;
time.tv_usec = 200000;
uint8_t *p_buff = NULL;
int buff_size = IW_SCAN_MAX_DATA;
bool is_end = false;
while(!is_end)
{
fd_set fds;
int ret;
FD_ZERO(&fds);
ret = select(0, &fds, NULL, NULL, &time);
if (ret == 0)
{
uint8_t *p_buff2;
while (!is_end)
{
p_buff2 = (uint8_t *)realloc(p_buff, buff_size);
p_buff = p_buff2;
w_request.u.data.pointer = p_buff;
w_request.u.data.flags = 0;
w_request.u.data.length = buff_size;
if (iw_get_ext(wlan_sock_, wlan_if_.c_str(), SIOCGIWSCAN, &w_request) < 0)
{
if (errno == E2BIG && range_.we_version_compiled > 16)
{
if (w_request.u.data.length > buff_size)
buff_size = w_request.u.data.length;
else
buff_size *= 2;
continue;
}
else if (errno == EAGAIN)
{
time.tv_sec = 0;
time.tv_usec = 200000;
break;
}
}
else
is_end = true;
}
}
}
// Put wifi data into ROS message
wifi_tools::AccessPoint ap;
iw_event event;
stream_descr stream;
wifi_stumble_.data.clear();
wifi_stumble_.header.stamp = ros::Time::now();
iw_init_event_stream(&stream, (char *)p_buff, w_request.u.data.length);
is_end = false;
int value = 0;
while(!is_end)
{
value = iw_extract_event_stream(&stream, &event, range_.we_version_compiled);
if(!(value>0))
{
is_end = true;
}
else
{
if(event.cmd == IWEVQUAL)
{
// quality part of statistics
//ROS_INFO("command=IWEVQUAL");
if (event.u.qual.level != 0 || (event.u.qual.updated & (IW_QUAL_DBM | IW_QUAL_RCPI)))
{
ap.noise = event.u.qual.noise;
ap.ss = event.u.qual.level;
}
}
else if(event.cmd == SIOCGIWAP)
{
// get access point MAC addresses
//ROS_INFO("command=SIOCGIWAP");
char mac_buff[1024];
iw_ether_ntop((const struct ether_addr *)&event.u.ap_addr.sa_data,mac_buff);
ap.mac_address = std::string(mac_buff);
ROS_INFO("mac_addr=%s",mac_buff);
}
else if(event.cmd == SIOCGIWESSID)
{
// get ESSID
//ROS_INFO("command=SIOCGIWESSID");
wifi_stumble_.data.push_back(ap);
}
else if(event.cmd == SIOCGIWENCODE)
{
// get encoding token & mode
//ROS_INFO("command=SIOCGIWENCODE");
}
else if(event.cmd == SIOCGIWFREQ)
{
// get channel/frequency (Hz)
//ROS_INFO("command=SIOCGIWFREQ");
}
else if(event.cmd == SIOCGIWRATE)
{
// get default bit rate (bps)
//ROS_INFO("command=SIOCGIWRATE");
}
else if(event.cmd == SIOCGIWMODE)
{
// get operation mode
//ROS_INFO("command=SIOCGIWMODE");
}
else if(event.cmd == IWEVCUSTOM)
{
// Driver specific ascii string
//ROS_INFO("command=IWEVCUSTOM");
}
else if(event.cmd == IWEVGENIE)
{
// Generic IE (WPA, RSN, WMM, ..)
//ROS_INFO("command=IWEVGENIE");
}
else
{
// another command
//ROS_INFO("another command");
}
}
}
checkDuplication();
return true;
}
// Function needs working on
void mapUpdate(double x_sens_dist, double y_sens_dist, int sensor)
{
//x_sens_dist and y_sens_dist are distance from robot to detected object.
// Always start at center of grid: add/subtract to center_x to all distances
// according to sign conventions.
// First, convert sensor readings from robot frame to map frame then and then convert
int x_sens_cell = floor(x_sens_dist/resolution);
int y_sens_cell = floor(y_sens_dist/resolution);
int weight_cell = 5;
double corner_x, corner_y;
int corner_x_cell, corner_y_cell;
tf::StampedTransform transform;
switch(sensor)
{
case 1:
try
{
listener.waitForTransform("/sensor1", "/map", ros::Time(0), ros::Duration(1));
listener.lookupTransform("/map", "/sensor1", ros::Time(0), transform);
corner_x = transform.getOrigin().getX();
corner_y = transform.getOrigin().getY();
corner_x_cell = floor(corner_x/resolution);
corner_y_cell = floor(corner_y/resolution);
ros::Time::init();
}
catch(tf::TransformException ex)
{
ROS_ERROR("%s",ex.what());
}
break;
case 2:
try
{
listener.waitForTransform("/sensor2", "/map", ros::Time(0), ros::Duration(1));
listener.lookupTransform("/map", "/sensor2", ros::Time(0), transform);
corner_x = transform.getOrigin().getX();
corner_y = transform.getOrigin().getY();
corner_x_cell = floor(corner_x/resolution);
corner_y_cell = floor(corner_y/resolution);
}
catch(tf::TransformException ex)
{
ROS_ERROR("%s",ex.what());
}
break;
case 3:
try
{
listener.waitForTransform("/sensor3", "/map", ros::Time(0), ros::Duration(1));
listener.lookupTransform("/map", "/sensor3", ros::Time(0), transform);
corner_x = transform.getOrigin().getX();
corner_y = transform.getOrigin().getY();
corner_x_cell = floor(corner_x/resolution);
corner_y_cell = floor(corner_y/resolution);
}
catch(tf::TransformException ex)
{
ROS_ERROR("%s",ex.what());
}
break;
case 4:
try
{
listener.waitForTransform("/sensor4", "/map", ros::Time(0), ros::Duration(1));
listener.lookupTransform("/map", "/sensor4", ros::Time(0), transform);
corner_x = transform.getOrigin().getX();
corner_y = transform.getOrigin().getY();
corner_x_cell = floor(corner_x/resolution);
corner_y_cell = floor(corner_y/resolution);
}
catch(tf::TransformException ex)
{
ROS_ERROR("%s",ex.what());
}
}
//map_vector[x_sens_cell+width_map*y_sens_cell]=100;
int x1, x2, y1, y2;
if (corner_x_cell > x_sens_cell) {
x1 = x_sens_cell;
x2 = corner_x_cell;
} else {
x1 = corner_x_cell;
x2 = x_sens_cell;
}
if (corner_y_cell > y_sens_cell) {
y1 = y_sens_cell;
y2 = corner_y_cell;
} else {
y1 = corner_y_cell;
y2 = y_sens_cell;
}
for(int i = x2 ; i >= x1; i--)
{
for(int j = y2 ; j >= y1; j--)
{
if((i==x_sens_cell) && (j==y_sens_cell))
{
if(map_vector[i+width_map*j] == -1)
map_vector[i+width_map*j] = 5;
else
map_vector[i+width_map*j] = map_vector[i+width_map*j] + weight_cell;
}
else
{
if(map_vector[i+width_map*j]==-1)
{
map_vector[i+width_map*j] = 0;
}
}
}
}
}
void
FootstepNavigation::executeFootsteps()
{
if (ivPlanner.getPathSize() <= 1)
return;
// lock this thread
ivExecutingFootsteps = true;
ROS_INFO("Start walking towards the goal.");
humanoid_nav_msgs::StepTarget step;
humanoid_nav_msgs::StepTargetService step_srv;
tf::Transform from;
std::string support_foot_id;
// calculate and perform relative footsteps until goal is reached
state_iter_t to_planned = ivPlanner.getPathBegin();
if (to_planned == ivPlanner.getPathEnd())
{
ROS_ERROR("No plan available. Return.");
return;
}
const State* from_planned = to_planned.base();
to_planned++;
while (to_planned != ivPlanner.getPathEnd())
{
try
{
boost::this_thread::interruption_point();
}
catch (const boost::thread_interrupted&)
{
// leave this thread
return;
}
if (from_planned->getLeg() == RIGHT)
support_foot_id = ivIdFootRight;
else // support_foot = LLEG
support_foot_id = ivIdFootLeft;
// try to get real placement of the support foot
if (getFootTransform(support_foot_id, ivIdMapFrame, ros::Time::now(),
ros::Duration(0.5), &from))
{
// calculate relative step and check if it can be performed
if (getFootstep(from, *from_planned, *to_planned, &step))
{
step_srv.request.step = step;
ivFootstepSrv.call(step_srv);
}
// ..if it cannot be performed initialize replanning
else
{
ROS_INFO("Footstep cannot be performed. Replanning necessary.");
replan();
// leave the thread
return;
}
}
else
{
// if the support foot could not be received wait and try again
ros::Duration(0.5).sleep();
continue;
}
from_planned = to_planned.base();
to_planned++;
}
ROS_INFO("Succeeded walking to the goal.\n");
// free the lock
ivExecutingFootsteps = false;
}
bool GazeboRosControllerManager::LoadControllerManagerFromURDF()
{
std::string urdf_string = GetURDF(this->robotParam);
// initialize TiXmlDocument doc with a string
TiXmlDocument doc;
if (!doc.Parse(urdf_string.c_str()) && doc.Error())
{
ROS_ERROR("Could not load the gazebo controller manager plugin's"
" configuration file: %s\n", urdf_string.c_str());
return false;
}
else
{
// debug
// doc.Print();
// std::cout << *(doc.RootElement()) << std::endl;
// Pulls out the list of actuators used in the robot configuration.
struct GetActuators : public TiXmlVisitor
{
std::set<std::string> actuators;
virtual bool VisitEnter(const TiXmlElement &elt, const TiXmlAttribute *)
{
if (elt.ValueStr() == std::string("actuator") && elt.Attribute("name"))
actuators.insert(elt.Attribute("name"));
else if (elt.ValueStr() ==
std::string("rightActuator") && elt.Attribute("name"))
actuators.insert(elt.Attribute("name"));
else if (elt.ValueStr() ==
std::string("leftActuator") && elt.Attribute("name"))
actuators.insert(elt.Attribute("name"));
return true;
}
} get_actuators;
doc.RootElement()->Accept(&get_actuators);
// Places the found actuators into the hardware interface.
std::set<std::string>::iterator it;
for (it = get_actuators.actuators.begin();
it != get_actuators.actuators.end(); ++it)
{
// std::cout << " adding actuator " << (*it) << std::endl;
pr2_hardware_interface::Actuator* pr2_actuator =
new pr2_hardware_interface::Actuator(*it);
pr2_actuator->state_.is_enabled_ = true;
this->hardware_interface_.addActuator(pr2_actuator);
}
// Setup mechanism control node
this->controller_manager_->initXml(doc.RootElement());
if (this->controller_manager_->state_ == NULL)
{
ROS_ERROR("Mechanism unable to parse robot_description URDF"
" to fill out robot state in controller_manager.");
return false;
}
// set fake calibration states for simulation
for (unsigned int i = 0;
i < this->controller_manager_->state_->joint_states_.size(); ++i)
this->controller_manager_->state_->joint_states_[i].calibrated_ =
calibration_status_;
return true;
}
}
boost::shared_ptr<kinematics::KinematicsBase> allocKinematicsSolver(const robot_model::JointModelGroup *jmg)
{
boost::shared_ptr<kinematics::KinematicsBase> result;
if (!jmg)
{
ROS_ERROR("Specified group is NULL. Cannot allocate kinematics solver.");
return result;
}
ROS_DEBUG("Received request to allocate kinematics solver for group '%s'", jmg->getName().c_str());
if (kinematics_loader_ && jmg)
{
std::map<std::string, std::vector<std::string> >::const_iterator it = possible_kinematics_solvers_.find(jmg->getName());
if (it != possible_kinematics_solvers_.end())
{
// just to be sure, do not call the same pluginlib instance allocation function in parallel
boost::mutex::scoped_lock slock(lock_);
for (std::size_t i = 0 ; !result && i < it->second.size() ; ++i)
{
try
{
result.reset(kinematics_loader_->createUnmanagedInstance(it->second[i]));
if (result)
{
const std::vector<const robot_model::LinkModel*> &links = jmg->getLinkModels();
if (!links.empty())
{
const std::string &base = links.front()->getParentJointModel()->getParentLinkModel() ?
links.front()->getParentJointModel()->getParentLinkModel()->getName() : jmg->getParentModel().getModelFrame();
// choose the tip of the IK solver
const std::vector<std::string> tips = chooseTipFrames(jmg);
// choose search resolution
double search_res = search_res_.find(jmg->getName())->second[i]; // we know this exists, by construction
if (!result->initialize(robot_description_, jmg->getName(),
(base.empty() || base[0] != '/') ? base : base.substr(1) , tips, search_res))
{
ROS_ERROR("Kinematics solver of type '%s' could not be initialized for group '%s'", it->second[i].c_str(), jmg->getName().c_str());
result.reset();
}
else
{
result->setDefaultTimeout(jmg->getDefaultIKTimeout());
ROS_DEBUG("Successfully allocated and initialized a kinematics solver of type '%s' with search resolution %lf for group '%s' at address %p",
it->second[i].c_str(), search_res, jmg->getName().c_str(), result.get());
}
}
else
ROS_ERROR("No links specified for group '%s'", jmg->getName().c_str());
}
}
catch (pluginlib::PluginlibException& e)
{
ROS_ERROR("The kinematics plugin (%s) failed to load. Error: %s", it->first.c_str(), e.what());
}
}
}
else
ROS_DEBUG("No kinematics solver available for this group");
}
if (!result)
{
ROS_DEBUG("No usable kinematics solver was found for this group.");
ROS_DEBUG("Did you load kinematics.yaml into your node's namespace?");
}
return result;
}
int main(int argc, char **argv)
{
int rate= 50;
float inv_rate=1/rate;
ros::init(argc, argv, "ARdrone_PID_to_Point");
ros::NodeHandle n;
ros::Rate loop_rate(rate);
tf::TransformListener tf_listener;
tf::TransformBroadcaster br;
tf::StampedTransform desired_pos;
tf::StampedTransform ardrone;
tf::StampedTransform trackee;
tf::StampedTransform desired_in_ardrone_coords;
ros::Publisher pub_twist;
geometry_msgs::Twist twist_msg;
tf::Quaternion ardrone_yawed;
memset(controls, 0, sizeof(double)*4);
memset(old_data, 0, sizeof(double)*5);
memset(new_data, 0, sizeof(double)*5);
memset(integration, 0, sizeof(double)*5);
memset(derivative, 0, sizeof(double)*5);
memset(pid, 0, sizeof(double)*4);
//PID params
min_control[yaw] =-1.0;
min_control[roll] =-1.0;
min_control[pitch]=-1.0;
min_control[thrust]=-1.0;
max_control[yaw]=1.0;
max_control[roll]=1.0;
max_control[pitch]=1.0;
max_control[thrust]=1.0;
min_pid =-5.0;
max_pid =5.0;
while (ros::ok())
{
try {
//Get desired position transform
tf_listener.waitForTransform("/optitrak", "/desired_position", ros::Time(0), ros::Duration(inv_rate));
tf_listener.lookupTransform("/optitrak", "/desired_position", ros::Time(0), desired_pos);
// Get the quad rotor transform
tf_listener.waitForTransform("/optitrak", "/ardrone", ros::Time(0), ros::Duration(inv_rate));
tf_listener.lookupTransform("/optitrak", "/ardrone", ros::Time(0), ardrone);
} catch (...) {
ROS_ERROR("Failed on initial transform: Check VRPN server");}
double y1, p1, r1;
btMatrix3x3(ardrone.getRotation()).getRPY(r1, p1, y1);
ardrone_yawed.setRPY(0.0,0.0,y1);
/* //Dep code
btQuaternion ardrone_yawed(y1, 0.0, 0.0);
*/
ardrone.setRotation(ardrone_yawed);
//set up twist publisher
pub_twist = n.advertise<geometry_msgs::Twist>("/cmd_vel", 1); /* Message queue length is just 1 */
// Register the ardrone without roll and pitch with the transform system
br.sendTransform( tf::StampedTransform(ardrone, ros::Time::now(), "/optitrak", "ardrone_wo_rp") );
try {
// Get the vector between quad without roll and pitch and the desired point
tf_listener.waitForTransform("/ardrone_wo_rp", "/desired", ros::Time(0), ros::Duration(inv_rate));
tf_listener.lookupTransform("/ardrone_wo_rp", "/desired", ros::Time(0), desired_in_ardrone_coords);
}
catch (...) {
ROS_ERROR("Failed on w/o roll and pitch transform");}
// Extract the yaw, x, y, z components
btMatrix3x3(desired_in_ardrone_coords.getRotation()).getRPY(r1, p1, y1);
new_data[yaw]=y1;
new_data[pitch] = desired_in_ardrone_coords.getOrigin().getX();
new_data[roll] = desired_in_ardrone_coords.getOrigin().getY();
new_data[thrust] = desired_in_ardrone_coords.getOrigin().getZ();
new_data[msg_time] = (double)ros::Time::now().toSec();
ROS_DEBUG("Error: [x: %f y: %f z: %f]", new_data[pitch], new_data[roll], new_data[thrust]);
// Integrate/Derivate the data
double deltaT = (new_data[msg_time] - old_data[msg_time]);
integration[yaw] += new_data[yaw] * deltaT;
integration[pitch] += new_data[pitch] * deltaT;
integration[roll] += new_data[roll] * deltaT;
integration[thrust] += new_data[thrust] * deltaT;
ROS_DEBUG("Integration: [deltaT: %f x: %f y: %f z: %f]", deltaT, integration[pitch], integration[roll], integration[thrust]);
derivative[yaw] = (new_data[yaw] - old_data[yaw])/deltaT;
derivative[pitch] = (new_data[pitch] - old_data[pitch])/deltaT;
derivative[roll] = (new_data[roll] - old_data[roll])/deltaT;
derivative[thrust] = (new_data[thrust] - old_data[thrust])/deltaT;
ROS_DEBUG("Derivative: [deltaT: %f x: %f y: %f z: %f]", deltaT, derivative[pitch], derivative[roll], derivative[thrust]);
// Calculate the PID values
pid[yaw] = K_p[yaw] * new_data[yaw] + K_d[yaw] * derivative[yaw] + K_i[yaw] * integration[yaw];
pid[pitch] = K_p[pitch] * new_data[pitch] + K_d[pitch] * derivative[pitch] + K_i[pitch] * integration[pitch];
pid[roll] = K_p[roll] * new_data[roll] + K_d[roll] * derivative[roll] + K_i[roll] * integration[roll];
pid[thrust] = K_p[thrust] * new_data[thrust] + K_d[thrust] * derivative[thrust] + K_i[thrust] * integration[thrust];
ROS_DEBUG("PID: [x: %f y: %f z: %f]", pid[pitch], pid[roll], pid[thrust]);
memcpy(old_data, new_data, sizeof(double)*5);
pid[yaw]=0.0; //YAW IS DISABLED!
controls[yaw] = map(pid[yaw], min_pid, max_pid, min_control[yaw], max_control[yaw]);
controls[pitch] = map(pid[pitch], min_pid, max_pid, min_control[roll], max_control[roll]);
controls[roll] = map(pid[roll], min_pid, max_pid, min_control[pitch], max_control[pitch]);
controls[thrust] = map(pid[thrust], min_pid, max_pid, min_control[thrust], max_control[thrust]);
ROS_DEBUG("Controls: [yaw: %f roll: %f pitch: %f thrust: %f]", controls[yaw], controls[roll], controls[pitch], controls[thrust]);
//change the ref frame to inverted x-y-z coords. by modifying the directional control
twist_msg.linear.x=-controls[roll];
twist_msg.linear.y=-controls[pitch];
twist_msg.linear.z=-controls[thrust];
twist_msg.angular.z=controls[yaw];
pub_twist.publish(twist_msg);
ros::spinOnce();
loop_rate.sleep();
}//while ros ok
ROS_ERROR("ROS::ok failed- Node Closing");
}//main
void ARSinglePublisher::getTransformationCallback (const sensor_msgs::ImageConstPtr & image_msg)
{
ARUint8 *dataPtr;
ARMarkerInfo *marker_info;
int marker_num;
int i, k;
/* Get the image from ROSTOPIC
* NOTE: the dataPtr format is BGR because the ARToolKit library was
* build with V4L, dataPtr format change according to the
* ARToolKit configure option (see config.h).*/
try
{
capture_ = bridge_.imgMsgToCv (image_msg, "bgr8");
}
catch (sensor_msgs::CvBridgeException & e)
{
ROS_ERROR ("Could not convert from '%s' to 'bgr8'.", image_msg->encoding.c_str ());
}
//cvConvertImage(capture_,capture_,CV_CVTIMG_FLIP); //flip image
dataPtr = (ARUint8 *) capture_->imageData;
// detect the markers in the video frame
if (arDetectMarker (dataPtr, threshold_, &marker_info, &marker_num) < 0)
{
ROS_FATAL ("arDetectMarker failed");
ROS_BREAK (); // FIXME: I don't think this should be fatal... -Bill
}
// check for known patterns
k = -1;
for (i = 0; i < marker_num; i++)
{
if (marker_info[i].id == patt_id_)
{
ROS_DEBUG ("Found pattern: %d ", patt_id_);
// make sure you have the best pattern (highest confidence factor)
if (k == -1)
k = i;
else if (marker_info[k].cf < marker_info[i].cf)
k = i;
}
}
if (k != -1)
{
// **** get the transformation between the marker and the real camera
double arQuat[4], arPos[3];
if (!useHistory_ || contF == 0)
arGetTransMat (&marker_info[k], marker_center_, markerWidth_, marker_trans_);
else
arGetTransMatCont (&marker_info[k], marker_trans_, marker_center_, markerWidth_, marker_trans_);
contF = 1;
//arUtilMatInv (marker_trans_, cam_trans);
arUtilMat2QuatPos (marker_trans_, arQuat, arPos);
// **** convert to ROS frame
double quat[4], pos[3];
pos[0] = arPos[0] * AR_TO_ROS;
pos[1] = arPos[1] * AR_TO_ROS;
pos[2] = arPos[2] * AR_TO_ROS;
quat[0] = -arQuat[0];
quat[1] = -arQuat[1];
quat[2] = -arQuat[2];
quat[3] = arQuat[3];
ROS_DEBUG (" QUAT: Pos x: %3.5f y: %3.5f z: %3.5f", pos[0], pos[1], pos[2]);
ROS_DEBUG (" Quat qx: %3.5f qy: %3.5f qz: %3.5f qw: %3.5f", quat[0], quat[1], quat[2], quat[3]);
// **** publish the marker
ar_pose_marker_.header.frame_id = image_msg->header.frame_id;
ar_pose_marker_.header.stamp = image_msg->header.stamp;
ar_pose_marker_.id = marker_info->id;
ar_pose_marker_.pose.pose.position.x = pos[0];
ar_pose_marker_.pose.pose.position.y = pos[1];
ar_pose_marker_.pose.pose.position.z = pos[2];
ar_pose_marker_.pose.pose.orientation.x = quat[0];
ar_pose_marker_.pose.pose.orientation.y = quat[1];
ar_pose_marker_.pose.pose.orientation.z = quat[2];
ar_pose_marker_.pose.pose.orientation.w = quat[3];
ar_pose_marker_.confidence = marker_info->cf;
arMarkerPub_.publish(ar_pose_marker_);
ROS_DEBUG ("Published ar_single marker");
// **** publish transform between camera and marker
btQuaternion rotation (quat[0], quat[1], quat[2], quat[3]);
btVector3 origin(pos[0], pos[1], pos[2]);
btTransform t(rotation, origin);
if(publishTf_)
{
if(reverse_transform_)
{
tf::StampedTransform markerToCam (t.inverse(), image_msg->header.stamp, markerFrame_.c_str(), image_msg->header.frame_id);
broadcaster_.sendTransform(markerToCam);
} else {
tf::StampedTransform camToMarker (t, image_msg->header.stamp, image_msg->header.frame_id, markerFrame_.c_str());
broadcaster_.sendTransform(camToMarker);
}
}
// **** publish visual marker
if(publishVisualMarkers_)
{
btVector3 markerOrigin(0, 0, 0.25 * markerWidth_ * AR_TO_ROS);
btTransform m(btQuaternion::getIdentity(), markerOrigin);
btTransform markerPose = t * m; // marker pose in the camera frame
tf::poseTFToMsg(markerPose, rvizMarker_.pose);
rvizMarker_.header.frame_id = image_msg->header.frame_id;
rvizMarker_.header.stamp = image_msg->header.stamp;
rvizMarker_.id = 1;
rvizMarker_.scale.x = 1.0 * markerWidth_ * AR_TO_ROS;
rvizMarker_.scale.y = 1.0 * markerWidth_ * AR_TO_ROS;
rvizMarker_.scale.z = 0.5 * markerWidth_ * AR_TO_ROS;
rvizMarker_.ns = "basic_shapes";
rvizMarker_.type = visualization_msgs::Marker::CUBE;
rvizMarker_.action = visualization_msgs::Marker::ADD;
rvizMarker_.color.r = 0.0f;
rvizMarker_.color.g = 1.0f;
rvizMarker_.color.b = 0.0f;
rvizMarker_.color.a = 1.0;
rvizMarker_.lifetime = ros::Duration();
rvizMarkerPub_.publish(rvizMarker_);
ROS_DEBUG ("Published visual marker");
}
}
else
{
contF = 0;
ROS_DEBUG ("Failed to locate marker");
}
}
void costmap_cb(const nav_msgs::GridCells::ConstPtr& msg) {
ODOM_FRAME = msg->header.frame_id;
geometry_msgs::PoseArray ff_msg;
ff_msg.header.stamp = msg->header.stamp;
ff_msg.header.frame_id = ODOM_FRAME;
last_fv = msg->header.stamp;
force_vector[0] = force_vector[1] = 0;
std::vector<geometry_msgs::Point> points = msg->cells;
std::vector<geometry_msgs::Pose> poses;
int count = 0;
float total_weight = 0.0;
for (std::vector<std::string>::size_type i = 0; i < points.size(); i++) {
geometry_msgs::PointStamped point, point_trans;
point.header.stamp = msg->header.stamp;
point.header.frame_id = ODOM_FRAME;
point.point.x = points[i].x;
point.point.y = points[i].y;
tf_listener->waitForTransform(BASE_FRAME, ODOM_FRAME, point.header.stamp, ros::Duration(0.1));
try {
tf_listener->transformPoint(BASE_FRAME, point, point_trans);
}
catch (tf::ExtrapolationException e) {
ROS_ERROR("Unable to get tf transform: %s", e.what());
return;
}
float x = point_trans.point.x;
float y = point_trans.point.y;
float yaw = atan(y / x);
if (x > 0) yaw = modulus(yaw + PI, 2*PI);
std::vector<float> point_vector = point_to_vector(x, y);
if (point_vector.size() > 0) {
count++;
//float weight = 0.75*(MAX_RANGE - BASE_RADIUS - CLEARING_DIST)/(sqrt(pow(x, 2) + pow(y, 2)) - BASE_RADIUS - CLEARING_DIST);
//if ( weight < 0 )
// std::cout << "evil" << std::endl;
//length += weight;
//float weight = (PI - abs(yaw)) / PI;
force_vector[0] += point_vector[0];
force_vector[1] += point_vector[1];
geometry_msgs::PoseStamped pose, pose_trans;
pose.header.stamp = msg->header.stamp;
pose.header.frame_id = BASE_FRAME;
pose.pose.position.x = x;
pose.pose.position.y = y;
pose.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
tf_listener->waitForTransform(ODOM_FRAME, BASE_FRAME, pose.header.stamp, ros::Duration(0.1));
try {
tf_listener->transformPose(ODOM_FRAME, pose, pose_trans);
}
catch (tf::ExtrapolationException e) {
ROS_ERROR("Unable to get tf transform: %s", e.what());
return;
}
poses.push_back(pose_trans.pose);
}
}
//force_vector[0] /= length + 1;
//force_vector[1] /= length + 1;
std::cout << force_vector[0] << "," << force_vector[1] << std::endl;
// publish resulting force vector as Pose
geometry_msgs::PoseStamped force, force_trans;
force.header.stamp = msg->header.stamp;
force.header.frame_id = BASE_FRAME;
float force_yaw = atan(force_vector[1] / force_vector[0]);
if (force_vector[0] > 0) force_yaw = modulus(force_yaw + PI, 2*PI);
force.pose.orientation = tf::createQuaternionMsgFromYaw(force_yaw);
tf_listener->waitForTransform(BASE_FRAME, ODOM_FRAME, msg->header.stamp, ros::Duration(0.1));
try {
tf_listener->transformPose(ODOM_FRAME, force, force_trans);
}
catch (tf::ExtrapolationException e) {
ROS_ERROR("Unable to get tf transform: %s", e.what());
return;
}
force_obst_pub.publish(force_trans);
// publish force field as PoseArray
ff_msg.poses = poses;
force_field_pub.publish(ff_msg);
}
int main(int argc, char **argv)
{
// Init the ROS node
ros::init (argc, argv, "move_right_arm_joint_goal_test");
// Precondition: Valid clock
ros::NodeHandle nh;
if (!ros::Time::waitForValid(ros::WallDuration(5.0))) // NOTE: Important when using simulated clock
{
ROS_FATAL("Timed-out waiting for valid time.");
return EXIT_FAILURE;
}
// Action client for sending motion planing requests
actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm("move_right_arm_torso", true);
// Wait for the action client to be connected to the server
ROS_INFO("Connecting to server...");
if (!move_arm.waitForServer(ros::Duration(5.0)))
{
ROS_ERROR("Timed-out waiting for the move_arm action server.");
return EXIT_FAILURE;
}
ROS_INFO("Connected to server.");
// Prepare motion plan request with joint-space goal
arm_navigation_msgs::MoveArmGoal goal;
std::vector<std::string> names(9);
names[0] = "torso_1_joint";
names[1] = "torso_2_joint";
names[2] = "arm_right_1_joint";
names[3] = "arm_right_2_joint";
names[4] = "arm_right_3_joint";
names[5] = "arm_right_4_joint";
names[6] = "arm_right_5_joint";
names[7] = "arm_right_6_joint";
names[8] = "arm_right_7_joint";
goal.motion_plan_request.group_name = "right_arm_torso";
goal.motion_plan_request.num_planning_attempts = 1;
goal.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
goal.motion_plan_request.planner_id= std::string("");
goal.planner_service_name = std::string("ompl_planning/plan_kinematic_path");
goal.motion_plan_request.goal_constraints.joint_constraints.resize(names.size());
for (unsigned int i = 0 ; i < goal.motion_plan_request.goal_constraints.joint_constraints.size(); ++i)
{
goal.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = names[i];
goal.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0;
goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.05;
goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.05;
}
goal.motion_plan_request.goal_constraints.joint_constraints[0].position = 0.0;
goal.motion_plan_request.goal_constraints.joint_constraints[1].position = 0.0;
goal.motion_plan_request.goal_constraints.joint_constraints[2].position = 1.2;
goal.motion_plan_request.goal_constraints.joint_constraints[3].position = 0.15;
goal.motion_plan_request.goal_constraints.joint_constraints[4].position = -1.5708;
goal.motion_plan_request.goal_constraints.joint_constraints[5].position = 1.3;
goal.motion_plan_request.goal_constraints.joint_constraints[6].position = 0.0;
goal.motion_plan_request.goal_constraints.joint_constraints[7].position = 0.0;
goal.motion_plan_request.goal_constraints.joint_constraints[8].position = 0.0;
// Send motion plan request
if (nh.ok())
{
bool finished_within_time = false;
move_arm.sendGoal(goal);
finished_within_time = move_arm.waitForResult(ros::Duration(15.0));
if (!finished_within_time)
{
move_arm.cancelGoal();
ROS_INFO("Timed out achieving joint-space goal.");
}
else
{
actionlib::SimpleClientGoalState state = move_arm.getState();
bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
if(success)
ROS_INFO("Action finished: %s",state.toString().c_str());
else
ROS_INFO("Action failed: %s",state.toString().c_str());
}
}
ros::shutdown();
}
void PclExtractor::cloudCallback(const Cloud2Msg::ConstPtr& cloud2Msg_input)
{
boost::mutex::scoped_lock(mutex_);
sensor_msgs::PointCloud2 output;
sensor_msgs::PointCloud2 convex_hull;
sensor_msgs::PointCloud2 object_msg;
sensor_msgs::PointCloud2 nonObject_msg;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg (*cloud2Msg_input, *cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_p(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f(new pcl::PointCloud<pcl::PointXYZ>);
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
// *** Normal estimation
// Create the normal estimation class and pass the input dataset to it
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud(cloud);
// Creating the kdTree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
ne.setSearchMethod(tree);
// output dataset
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
// use all neighbors in a sphere of radius 3cm
ne.setRadiusSearch(0.3);
// compute the features
ne.compute(*cloud_normals);
// *** End of normal estimation
// *** Plane Estimation From Normals Start
// Create the segmentation object
pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> seg;
// Optional
seg.setOptimizeCoefficients(true);
// Mandatory
// seg.setModelType(pcl::SACMODEL_PARALLEL_PLANE);
seg.setModelType(pcl::SACMODEL_PLANE);
// seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE);
//y가 z
// const Eigen::Vector3f z_axis(0,-1.0,0);
// seg.setAxis(z_axis);
// seg.setEpsAngle(seg_setEpsAngle_);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setMaxIterations (seg_setMaxIterations_);
seg.setDistanceThreshold(seg_setDistanceThreshold_);
seg.setNormalDistanceWeight(seg_setNormalDistanceWeight_);
// seg.setProbability(seg_probability_);
seg.setInputCloud((*cloud).makeShared());
seg.setInputNormals(cloud_normals);
seg.segment(*inliers, *coefficients);
std::cerr <<"input: "<<cloud->width*cloud->height<<"Model inliers: " << inliers->indices.size () << std::endl;
if (inliers->indices.size () == 0)
{
ROS_ERROR ("Could not estimate a planar model for the given dataset.");
}
// *** End of Plane Estimation
// *** Plane Estimation Start
// Create the segmentation object
/* pcl::SACSegmentation<pcl::PointXYZ> seg;
// Optional
//seg.setOptimizeCoefficients(true);
// Mandatory
seg.setModelType(pcl::SACMODEL_PARALLEL_PLANE);
// seg.setModelType(pcl::SACMODEL_PLANE);
// seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE);
//y가 z
const Eigen::Vector3f z_axis(0,-1.0,0);
seg.setAxis(z_axis);
seg.setEpsAngle(seg_setEpsAngle_);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setMaxIterations (seg_setMaxIterations_);
seg.setDistanceThreshold(seg_setDistanceThreshold_);
seg.setInputCloud((*cloud).makeShared());
seg.segment(*inliers, *coefficients);
std::cerr <<"input: "<<cloud->width*cloud->height<<"Model inliers: " << inliers->indices.size () << std::endl;
if (inliers->indices.size () == 0)
{
ROS_ERROR ("Could not estimate a planar model for the given dataset.");
}
// *** End of Plane Estimation
*/
pcl::ExtractIndices<pcl::PointXYZ> extract;
// Extrat the inliers
extract.setInputCloud(cloud);
extract.setIndices(inliers);
pcl::PointCloud<pcl::PointXYZ>::Ptr ground_hull(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected(new pcl::PointCloud<pcl::PointXYZ>);
if ((int)inliers->indices.size() > minPoints_)
{
extract.setNegative(false);
extract.filter(*cloud_p);
pcl::toROSMsg(*cloud_p, output);
std::cerr << "PointCloud representing the planar component: "
<< cloud_p->width * cloud_p->height << " data points." << std::endl;
// Step 3c. Project the ground inliers
pcl::ProjectInliers<pcl::PointXYZ> proj;
proj.setModelType(pcl::SACMODEL_PLANE);
proj.setInputCloud(cloud_p);
proj.setModelCoefficients(coefficients);
proj.filter(*cloud_projected);
// Step 3d. Create a Convex Hull representation of the projected inliers
pcl::ConvexHull<pcl::PointXYZ> chull;
//chull.setInputCloud(cloud_p);
chull.setInputCloud(cloud_projected);
chull.setDimension(chull_setDimension_);//2D
chull.reconstruct(*ground_hull);
pcl::toROSMsg(*ground_hull, convex_hull);
//pcl::toROSMsg(*ground_hull, convex_hull);
ROS_INFO ("Convex hull has: %d data points.", (int) ground_hull->points.size ());
// Publish the data
plane_pub_.publish (output);
hull_pub_.publish(convex_hull);
}
// Create the filtering object
extract.setNegative(true);
extract.filter(*cloud_f);
ROS_INFO ("Cloud_f has: %d data points.", (int) cloud_f->points.size ());
// cloud.swap(cloud_f);
pcl::PointCloud<pcl::PointXYZ>::Ptr object(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr nonObject(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointIndices::Ptr object_indices(new pcl::PointIndices);
// cloud statictical filter
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudStatisticalFiltered (new pcl::PointCloud<pcl::PointXYZ>);
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
sor.setInputCloud(cloud_f);
sor.setMeanK(sor_setMeanK_);
sor.setStddevMulThresh(sor_setStddevMulThresh_);
sor.filter(*cloudStatisticalFiltered);
pcl::ExtractIndices<pcl::PointXYZ> exObjectIndices;
//exObjectIndices.setInputCloud(cloud_f);
exObjectIndices.setInputCloud(cloudStatisticalFiltered);
exObjectIndices.setIndices(object_indices);
exObjectIndices.filter(*object);
exObjectIndices.setNegative(true);
exObjectIndices.filter(*nonObject);
ROS_INFO ("Object has: %d data points.", (int) object->points.size ());
pcl::toROSMsg(*object, object_msg);
object_pub_.publish(object_msg);
//pcl::toROSMsg(*nonObject, nonObject_msg);
//pcl::toROSMsg(*cloud_f, nonObject_msg);
pcl::toROSMsg(*cloudStatisticalFiltered, nonObject_msg);
nonObject_pub_.publish(nonObject_msg);
}
void Sub8StereoHandler::generate_point_cloud(
const sensor_msgs::ImageConstPtr &msg_l,
const sensor_msgs::ImageConstPtr &msg_r,
pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pcl_ptr, ros::Publisher &pc_pub) {
// NOTE: Calls to this function will block until the Synchronizer is able to
// secure
// the corresponding messages.
float px, py, pz;
cv_bridge::CvImagePtr cv_ptr_l;
cv_bridge::CvImagePtr cv_ptr_r;
cv_bridge::CvImagePtr cv_ptr_bgr;
try {
cv_ptr_l = cv_bridge::toCvCopy(msg_l, sensor_msgs::image_encodings::MONO8);
cv_ptr_r = cv_bridge::toCvCopy(msg_r, sensor_msgs::image_encodings::MONO8);
// Used to reproject color onto pointcloud (We'll just use the left image)
cv_ptr_bgr = cv_bridge::toCvCopy(msg_l, sensor_msgs::image_encodings::BGR8);
} catch (cv_bridge::Exception &e) {
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
cv::gpu::Stream pc_stream = cv::gpu::Stream();
this->d_left.upload(cv_ptr_l->image);
this->d_right.upload(cv_ptr_r->image);
switch (this->matching_method) {
case 0:
if (this->d_left.channels() > 1 || this->d_right.channels() > 1) {
// TODO: Add handle for these cases
ROS_WARN("Block-matcher does not support color images");
}
this->bm(this->d_left, this->d_right, this->d_disp, pc_stream);
break; // BM
case 1:
this->bp(this->d_left, this->d_right, this->d_disp, pc_stream);
break; // BP
case 2:
this->csbp(this->d_left, this->d_right, this->d_disp, pc_stream);
break; // CSBP
}
// cv::Mat h_disp, h_3d_img, h_point;
cv::gpu::reprojectImageTo3D(this->d_disp, this->gpu_pdisp, this->Q_, 4,
pc_stream);
this->gpu_pdisp.download(this->pdisp);
for (int i = 0; i < this->pdisp.rows; i++) {
// For future reference OpenCV is ROW-MAJOR
for (int j = 0; j < this->pdisp.cols; j++) {
cv::Point3f ptxyz = this->pdisp.at<cv::Point3f>(i, j);
if (!isinf(ptxyz.x) && !isinf(ptxyz.y) && !isinf(ptxyz.z)) {
// Reproject color points onto distances
cv::Vec3b color_info = cv_ptr_bgr->image.at<cv::Vec3b>(i, j);
pcl::PointXYZRGB pcl_pt;
pcl_pt.x = ptxyz.x;
pcl_pt.y = ptxyz.y;
pcl_pt.z = ptxyz.z;
uint32_t rgb = (static_cast<uint32_t>(color_info[2]) << 16 |
static_cast<uint32_t>(color_info[1]) << 8 |
static_cast<uint32_t>(color_info[0]));
pcl_pt.rgb = *reinterpret_cast<float *>(&rgb);
pcl_ptr->points.push_back(pcl_pt);
}
}
}
pcl_ptr->width = (int)pcl_ptr->points.size();
pcl_ptr->height = 1;
pcl::toROSMsg(*pcl_ptr, this->stereo_pc);
this->stereo_pc.header.frame_id = "/stereo_front";
pcl_ptr->clear();
pc_pub.publish(this->stereo_pc);
}
void ARMultiPublisher::getTransformationCallback (const sensor_msgs::ImageConstPtr & image_msg)
{
ARUint8 *dataPtr;
ARMarkerInfo *marker_info;
int marker_num;
int i, k, j;
/* Get the image from ROSTOPIC
* NOTE: the dataPtr format is BGR because the ARToolKit library was
* build with V4L, dataPtr format change according to the
* ARToolKit configure option (see config.h).*/
#if ROS_VERSION_MINIMUM(1, 9, 0)
try
{
capture_ = cv_bridge::toCvCopy (image_msg, sensor_msgs::image_encodings::BGR8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
}
dataPtr = (ARUint8 *) ((IplImage) capture_->image).imageData;
#else
try
{
capture_ = bridge_.imgMsgToCv (image_msg, "bgr8");
}
catch (sensor_msgs::CvBridgeException & e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
}
dataPtr = (ARUint8 *) capture_->imageData;
#endif
// detect the markers in the video frame
if (arDetectMarker (dataPtr, threshold_, &marker_info, &marker_num) < 0)
{
argCleanup ();
ROS_BREAK ();
}
arPoseMarkers_.markers.clear ();
// check for known patterns
for (i = 0; i < objectnum; i++)
{
k = -1;
for (j = 0; j < marker_num; j++)
{
if (object[i].id == marker_info[j].id)
{
if (k == -1)
k = j;
else // make sure you have the best pattern (highest confidence factor)
if (marker_info[k].cf < marker_info[j].cf)
k = j;
}
}
if (k == -1)
{
object[i].visible = 0;
continue;
}
// calculate the transform for each marker
if (object[i].visible == 0)
{
arGetTransMat (&marker_info[k], object[i].marker_center, object[i].marker_width, object[i].trans);
}
else
{
arGetTransMatCont (&marker_info[k], object[i].trans,
object[i].marker_center, object[i].marker_width, object[i].trans);
}
object[i].visible = 1;
double arQuat[4], arPos[3];
//arUtilMatInv (object[i].trans, cam_trans);
arUtilMat2QuatPos (object[i].trans, arQuat, arPos);
// **** convert to ROS frame
double quat[4], pos[3];
pos[0] = arPos[0] * AR_TO_ROS;
pos[1] = arPos[1] * AR_TO_ROS;
pos[2] = arPos[2] * AR_TO_ROS;
if (isRightCamera_) {
pos[2] += 0; // -0.001704;
pos[0] += 0; // +0.0899971;
pos[1] += 0; // -0.00012;
}
quat[0] = -arQuat[0];
quat[1] = -arQuat[1];
quat[2] = -arQuat[2];
quat[3] = arQuat[3];
ROS_DEBUG (" Object num %i ------------------------------------------------", i);
ROS_DEBUG (" QUAT: Pos x: %3.5f y: %3.5f z: %3.5f", pos[0], pos[1], pos[2]);
ROS_DEBUG (" Quat qx: %3.5f qy: %3.5f qz: %3.5f qw: %3.5f", quat[0], quat[1], quat[2], quat[3]);
// **** publish the marker
ar_pose::ARMarker ar_pose_marker;
ar_pose_marker.header.frame_id = image_msg->header.frame_id;
ar_pose_marker.header.stamp = image_msg->header.stamp;
ar_pose_marker.id = object[i].id;
ar_pose_marker.pose.pose.position.x = pos[0];
ar_pose_marker.pose.pose.position.y = pos[1];
ar_pose_marker.pose.pose.position.z = pos[2];
ar_pose_marker.pose.pose.orientation.x = quat[0];
ar_pose_marker.pose.pose.orientation.y = quat[1];
ar_pose_marker.pose.pose.orientation.z = quat[2];
ar_pose_marker.pose.pose.orientation.w = quat[3];
ar_pose_marker.confidence = round(marker_info[k].cf * 100);
arPoseMarkers_.markers.push_back (ar_pose_marker);
// **** publish transform between camera and marker
#if ROS_VERSION_MINIMUM(1, 9, 0)
btQuaternion rotation (quat[0], quat[1], quat[2], quat[3]);
btVector3 origin (pos[0], pos[1], pos[2]);
btTransform t (rotation, origin);
#else
// DEPRECATED: Fuerte support ends when Hydro is released
btQuaternion rotation (quat[0], quat[1], quat[2], quat[3]);
btVector3 origin (pos[0], pos[1], pos[2]);
btTransform t (rotation, origin);
#endif
if (publishTf_)
{
std::string name = object[i].name;
if (isRightCamera_) {
name += "_r";
}
tf::StampedTransform camToMarker (t, image_msg->header.stamp, image_msg->header.frame_id, name);
broadcaster_.sendTransform(camToMarker);
}
// **** publish visual marker
if (publishVisualMarkers_)
{
#if ROS_VERSION_MINIMUM(1, 9, 0)
btVector3 markerOrigin (0, 0, 0.25 * object[i].marker_width * AR_TO_ROS);
btTransform m (btQuaternion::getIdentity (), markerOrigin);
btTransform markerPose = t * m; // marker pose in the camera frame
#else
// DEPRECATED: Fuerte support ends when Hydro is released
btVector3 markerOrigin (0, 0, 0.25 * object[i].marker_width * AR_TO_ROS);
btTransform m (btQuaternion::getIdentity (), markerOrigin);
btTransform markerPose = t * m; // marker pose in the camera frame
#endif
tf::poseTFToMsg (markerPose, rvizMarker_.pose);
rvizMarker_.header.frame_id = image_msg->header.frame_id;
rvizMarker_.header.stamp = image_msg->header.stamp;
rvizMarker_.id = object[i].id;
rvizMarker_.scale.x = 1.0 * object[i].marker_width * AR_TO_ROS;
rvizMarker_.scale.y = 1.0 * object[i].marker_width * AR_TO_ROS;
rvizMarker_.scale.z = 0.5 * object[i].marker_width * AR_TO_ROS;
rvizMarker_.ns = "basic_shapes";
rvizMarker_.type = visualization_msgs::Marker::CUBE;
rvizMarker_.action = visualization_msgs::Marker::ADD;
switch (i)
{
case 0:
rvizMarker_.color.r = 0.0f;
rvizMarker_.color.g = 0.0f;
rvizMarker_.color.b = 1.0f;
rvizMarker_.color.a = 1.0;
break;
case 1:
rvizMarker_.color.r = 1.0f;
rvizMarker_.color.g = 0.0f;
rvizMarker_.color.b = 0.0f;
rvizMarker_.color.a = 1.0;
break;
default:
rvizMarker_.color.r = 0.0f;
rvizMarker_.color.g = 1.0f;
rvizMarker_.color.b = 0.0f;
rvizMarker_.color.a = 1.0;
}
rvizMarker_.lifetime = ros::Duration (1.0);
rvizMarkerPub_.publish(rvizMarker_);
ROS_DEBUG ("Published visual marker");
}
}
arMarkerPub_.publish(arPoseMarkers_);
ROS_DEBUG ("Published ar_multi markers");
}
void JointStateController::addExtraJoints(const ros::NodeHandle& nh, sensor_msgs::JointState& msg)
{
// Preconditions
XmlRpc::XmlRpcValue list;
if (!nh.getParam("extra_joints", list))
{
ROS_DEBUG("No extra joints specification found.");
return;
}
if (list.getType() != XmlRpc::XmlRpcValue::TypeArray)
{
ROS_ERROR("Extra joints specification is not an array. Ignoring.");
return;
}
for(std::size_t i = 0; i < list.size(); ++i)
{
if (list[i].getType() != XmlRpc::XmlRpcValue::TypeStruct)
{
ROS_ERROR_STREAM("Extra joint specification is not a struct, but rather '" << list[i].getType() <<
"'. Ignoring.");
continue;
}
if (!list[i].hasMember("name"))
{
ROS_ERROR_STREAM("Extra joint does not specify name. Ignoring.");
continue;
}
const std::string name = list[i]["name"];
if (std::find(msg.name.begin(), msg.name.end(), name) != msg.name.end())
{
ROS_WARN_STREAM("Joint state interface already contains specified extra joint '" << name << "'.");
continue;
}
const bool has_pos = list[i].hasMember("position");
const bool has_vel = list[i].hasMember("velocity");
const bool has_eff = list[i].hasMember("effort");
const XmlRpc::XmlRpcValue::Type typeDouble = XmlRpc::XmlRpcValue::TypeDouble;
if (has_pos && list[i]["position"].getType() != typeDouble)
{
ROS_ERROR_STREAM("Extra joint '" << name << "' does not specify a valid default position. Ignoring.");
continue;
}
if (has_vel && list[i]["velocity"].getType() != typeDouble)
{
ROS_ERROR_STREAM("Extra joint '" << name << "' does not specify a valid default velocity. Ignoring.");
continue;
}
if (has_eff && list[i]["effort"].getType() != typeDouble)
{
ROS_ERROR_STREAM("Extra joint '" << name << "' does not specify a valid default effort. Ignoring.");
continue;
}
// State of extra joint
const double pos = has_pos ? static_cast<double>(list[i]["position"]) : 0.0;
const double vel = has_vel ? static_cast<double>(list[i]["velocity"]) : 0.0;
const double eff = has_eff ? static_cast<double>(list[i]["effort"]) : 0.0;
// Add extra joints to message
msg.name.push_back(name);
msg.position.push_back(pos);
msg.velocity.push_back(vel);
msg.effort.push_back(eff);
}
}
// Constructor
NodeClass(std::string name):
as_(n_, name, boost::bind(&NodeClass::executeCB, this, _1)),
action_name_(name)
{
n_ = ros::NodeHandle("~");
isInitialized_ = false;
isError_ = false;
ActualPos_=0.0;
ActualVel_=0.0;
CamAxis_ = new ElmoCtrl();
CamAxisParams_ = new ElmoCtrlParams();
// implementation of topics to publish
topicPub_JointState_ = n_.advertise<sensor_msgs::JointState>("/joint_states", 1);
topicPub_ControllerState_ = n_.advertise<pr2_controllers_msgs::JointTrajectoryControllerState>("state", 1);
topicPub_Diagnostic_ = n_.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 1);
// implementation of topics to subscribe
// implementation of service servers
srvServer_Init_ = n_.advertiseService("init", &NodeClass::srvCallback_Init, this);
srvServer_Stop_ = n_.advertiseService("stop", &NodeClass::srvCallback_Stop, this);
srvServer_Recover_ = n_.advertiseService("recover", &NodeClass::srvCallback_Recover, this);
srvServer_SetOperationMode_ = n_.advertiseService("set_operation_mode", &NodeClass::srvCallback_SetOperationMode, this);
srvServer_SetDefaultVel_ = n_.advertiseService("set_default_vel", &NodeClass::srvCallback_SetDefaultVel, this);
// implementation of service clients
//--
// read parameters from parameter server
if(!n_.hasParam("EnoderIncrementsPerRevMot")) ROS_WARN("cob_head_axis: couldn't find parameter EnoderIncrementsPerRevMot, check if ALL parameters have been set correctly");
n_.param<std::string>("CanDevice", CanDevice_, "PCAN");
n_.param<int>("CanBaudrate", CanBaudrate_, 500);
n_.param<int>("HomingDir", HomingDir_, 1);
n_.param<int>("HomingDigIn", HomingDigIn_, 11);
n_.param<int>("ModId",ModID_, 17);
n_.param<std::string>("JointName",JointName_, "head_axis_joint");
n_.param<std::string>("CanIniFile",CanIniFile_, "/");
n_.param<std::string>("operation_mode",operationMode_, "position");
n_.param<int>("MotorDirection",MotorDirection_, 1);
n_.param<double>("GearRatio",GearRatio_, 62.5);
n_.param<int>("EnoderIncrementsPerRevMot",EnoderIncrementsPerRevMot_, 4096);
ROS_INFO("CanDevice=%s, CanBaudrate=%d, ModID=%d, HomingDigIn=%d",CanDevice_.c_str(),CanBaudrate_,ModID_,HomingDigIn_);
// load parameter server string for robot/model
std::string param_name = "/robot_description";
std::string full_param_name;
std::string xml_string;
n_.searchParam(param_name,full_param_name);
n_.getParam(full_param_name.c_str(),xml_string);
ROS_INFO("full_param_name=%s",full_param_name.c_str());
if (xml_string.size()==0)
{
ROS_ERROR("Unable to load robot model from param server robot_description\n");
exit(2);
}
ROS_DEBUG("%s content\n%s", full_param_name.c_str(), xml_string.c_str());
// extract limits and velocitys from urdf model
urdf::Model model;
if (!model.initString(xml_string))
{
ROS_ERROR("Failed to parse urdf file");
exit(2);
}
ROS_INFO("Successfully parsed urdf file");
// get LowerLimits out of urdf model
LowerLimit_ = model.getJoint(JointName_.c_str())->limits->lower;
//std::cout << "LowerLimits[" << JointNames[i].c_str() << "] = " << LowerLimits[i] << std::endl;
CamAxisParams_->SetLowerLimit(LowerLimit_);
// get UpperLimits out of urdf model
UpperLimit_ = model.getJoint(JointName_.c_str())->limits->upper;
//std::cout << "LowerLimits[" << JointNames[i].c_str() << "] = " << LowerLimits[i] << std::endl;
CamAxisParams_->SetUpperLimit(UpperLimit_);
// get Offset out of urdf model
Offset_ = model.getJoint(JointName_.c_str())->calibration->rising.get()[0];
//std::cout << "Offset[" << JointNames[i].c_str() << "] = " << Offsets[i] << std::endl;
CamAxisParams_->SetAngleOffset(Offset_);
// get velocitiy out of urdf model
MaxVel_ = model.getJoint(JointName_.c_str())->limits->velocity;
ROS_INFO("Successfully read limits from urdf");
//initializing and homing of camera_axis
CamAxisParams_->SetCanIniFile(CanIniFile_);
CamAxisParams_->SetHomingDir(HomingDir_);
CamAxisParams_->SetHomingDigIn(HomingDigIn_);
CamAxisParams_->SetMaxVel(MaxVel_);
CamAxisParams_->SetGearRatio(GearRatio_);
CamAxisParams_->SetMotorDirection(MotorDirection_);
CamAxisParams_->SetEncoderIncrements(EnoderIncrementsPerRevMot_);
CamAxisParams_->Init(CanDevice_, CanBaudrate_, ModID_);
}
int RosAriaNode::Setup()
{
// Note, various objects are allocated here which are never deleted (freed), since Setup() is only supposed to be
// called once per instance, and these objects need to persist until the process terminates.
robot = new ArRobot();
ArArgumentBuilder *args = new ArArgumentBuilder(); // never freed
ArArgumentParser *argparser = new ArArgumentParser(args); // Warning never freed
argparser->loadDefaultArguments(); // adds any arguments given in /etc/Aria.args. Useful on robots with unusual serial port or baud rate (e.g. pioneer lx)
// Now add any parameters given via ros params (see RosAriaNode constructor):
// if serial port parameter contains a ':' character, then interpret it as hostname:tcpport
// for wireless serial connection. Otherwise, interpret it as a serial port name.
size_t colon_pos = serial_port.find(":");
if (colon_pos != std::string::npos)
{
args->add("-remoteHost"); // pass robot's hostname/IP address to Aria
args->add(serial_port.substr(0, colon_pos).c_str());
args->add("-remoteRobotTcpPort"); // pass robot's TCP port to Aria
args->add(serial_port.substr(colon_pos+1).c_str());
}
else
{
args->add("-robotPort"); // pass robot's serial port to Aria
args->add(serial_port.c_str());
}
// if a baud rate was specified in baud parameter
if(serial_baud != 0)
{
args->add("-robotBaud");
char tmp[100];
snprintf(tmp, 100, "%d", serial_baud);
args->add(tmp);
}
if( debug_aria )
{
// turn on all ARIA debugging
args->add("-robotLogPacketsReceived"); // log received packets
args->add("-robotLogPacketsSent"); // log sent packets
args->add("-robotLogVelocitiesReceived"); // log received velocities
args->add("-robotLogMovementSent");
args->add("-robotLogMovementReceived");
ArLog::init(ArLog::File, ArLog::Verbose, aria_log_filename.c_str(), true);
}
// Connect to the robot
conn = new ArRobotConnector(argparser, robot); // warning never freed
if (!conn->connectRobot()) {
ROS_ERROR("RosAria: ARIA could not connect to robot! (Check ~port parameter is correct, and permissions on port device.)");
return 1;
}
// causes ARIA to load various robot-specific hardware parameters from the robot parameter file in /usr/local/Aria/params
if(!Aria::parseArgs())
{
ROS_ERROR("RosAria: ARIA error parsing ARIA startup parameters!");
return 1;
}
// Start dynamic_reconfigure server
dynamic_reconfigure_server = new dynamic_reconfigure::Server<rosaria::RosAriaConfig>;
robot->lock();
// Setup Parameter Minimums
rosaria::RosAriaConfig dynConf_min;
//arbitrary non-zero values so dynamic reconfigure isn't STUPID
dynConf_min.trans_vel_max = 0.1;
dynConf_min.rot_vel_max = 0.1;
dynConf_min.trans_accel = 0.1;
dynConf_min.trans_decel = 0.1;
dynConf_min.rot_accel = 0.1;
dynConf_min.rot_decel = 0.1;
// I'm setting these upper bounds relitivly arbitrarily, feel free to increase them.
dynConf_min.TicksMM = 10;
dynConf_min.DriftFactor = -200;
dynConf_min.RevCount = -32760;
dynamic_reconfigure_server->setConfigMin(dynConf_min);
rosaria::RosAriaConfig dynConf_max;
dynConf_max.trans_vel_max = robot->getAbsoluteMaxTransVel() / 1000.0;
dynConf_max.rot_vel_max = robot->getAbsoluteMaxRotVel() *M_PI/180.0;
dynConf_max.trans_accel = robot->getAbsoluteMaxTransAccel() / 1000.0;
dynConf_max.trans_decel = robot->getAbsoluteMaxTransDecel() / 1000.0;
dynConf_max.rot_accel = robot->getAbsoluteMaxRotAccel() * M_PI/180.0;
dynConf_max.rot_decel = robot->getAbsoluteMaxRotDecel() * M_PI/180.0;
// I'm setting these upper bounds relitivly arbitrarily, feel free to increase them.
dynConf_max.TicksMM = 200;
dynConf_max.DriftFactor = 200;
dynConf_max.RevCount = 32760;
dynamic_reconfigure_server->setConfigMax(dynConf_max);
dynConf_default.trans_vel_max = robot->getTransVelMax() / 1000.0;
dynConf_default.rot_vel_max = robot->getRotVelMax() *M_PI/180.0;
dynConf_default.trans_accel = robot->getTransAccel() / 1000.0;
dynConf_default.trans_decel = robot->getTransDecel() / 1000.0;
dynConf_default.rot_accel = robot->getRotAccel() * M_PI/180.0;
dynConf_default.rot_decel = robot->getRotDecel() * M_PI/180.0;
/* ROS_ERROR("ON ROBOT NOW\n\
Trans vel max: %f\n\
Rot vel max: %f\n\
\n\
trans accel: %f\n\
trans decel: %f\n\
rot accel: %f\n\
rot decel: %f", robot->getTransVelMax(), robot->getRotVelMax(), robot->getTransAccel(), robot->getTransDecel(), robot->getRotAccel(), robot->getRotDecel());
ROS_ERROR("IN DEFAULT CONFIG\n\
Trans vel max: %f\n\
Rot vel max: %f\n\
\n\
trans accel: %f\n\
trans decel: %f\n\
rot accel: %f\n\
rot decel: %f\n", dynConf_default.trans_vel_max, dynConf_default.rot_vel_max, dynConf_default.trans_accel, dynConf_default.trans_decel, dynConf_default.rot_accel, dynConf_default.rot_decel);*/
TicksMM = robot->getOrigRobotConfig()->getTicksMM();
DriftFactor = robot->getOrigRobotConfig()->getDriftFactor();
RevCount = robot->getOrigRobotConfig()->getRevCount();
dynConf_default.TicksMM = TicksMM;
dynConf_default.DriftFactor = DriftFactor;
dynConf_default.RevCount = RevCount;
dynamic_reconfigure_server->setConfigDefault(dynConf_default);
for(int i = 0; i < 16; i++)
{
sonar_tf_array[i].header.frame_id = frame_id_base_link;
std::stringstream _frame_id;
_frame_id << "sonar" << i;
sonar_tf_array[i].child_frame_id = _frame_id.str();
ArSensorReading* _reading = NULL;
_reading = robot->getSonarReading(i);
sonar_tf_array[i].transform.translation.x = _reading->getSensorX() / 1000.0;
sonar_tf_array[i].transform.translation.y = _reading->getSensorY() / 1000.0;
sonar_tf_array[i].transform.translation.z = 0.19;
sonar_tf_array[i].transform.rotation = tf::createQuaternionMsgFromYaw(_reading->getSensorTh() * M_PI / 180.0);
}
for (int i=0;i<16;i++) {
sensor_msgs::Range r;
ranges.data.push_back(r);
}
int i=0,j=0;
if (sonars__crossed_the_streams) {
i=8;
j=8;
}
for(; i<16; i++) {
//populate the RangeArray msg
std::stringstream _frame_id;
_frame_id << "sonar" << i;
ranges.data[i].header.frame_id = _frame_id.str();
ranges.data[i].radiation_type = 0;
ranges.data[i].field_of_view = 0.2618f;
ranges.data[i].min_range = 0.03f;
ranges.data[i].max_range = 5.0f;
}
// Enable the motors
robot->enableMotors();
robot->disableSonar();
// Initialize bumpers with robot number of bumpers
bumpers.front_bumpers.resize(robot->getNumFrontBumpers());
bumpers.rear_bumpers.resize(robot->getNumRearBumpers());
robot->unlock();
pose_pub = n.advertise<nav_msgs::Odometry>("pose",1000);
bumpers_pub = n.advertise<rosaria::BumperState>("bumper_state",1000);
voltage_pub = n.advertise<std_msgs::Float64>("battery_voltage", 1000);
combined_range_pub = n.advertise<rosaria::RangeArray>("ranges", 1000,
boost::bind(&RosAriaNode::sonarConnectCb,this),
boost::bind(&RosAriaNode::sonarDisconnectCb, this));
for(int i =0; i < 16; i++) {
std::stringstream topic_name;
topic_name << "range" << i;
range_pub[i] = n.advertise<sensor_msgs::Range>(topic_name.str().c_str(), 1000,
boost::bind(&RosAriaNode::sonarConnectCb,this),
boost::bind(&RosAriaNode::sonarDisconnectCb, this));
}
recharge_state_pub = n.advertise<std_msgs::Int8>("battery_recharge_state", 5, true /*latch*/ );
recharge_state.data = -2;
state_of_charge_pub = n.advertise<std_msgs::Float32>("battery_state_of_charge", 100);
motors_state_pub = n.advertise<std_msgs::Bool>("motors_state", 5, true /*latch*/ );
motors_state.data = false;
published_motors_state = false;
// subscribe to services
cmdvel_sub = n.subscribe( "cmd_vel", 1, (boost::function <void(const geometry_msgs::TwistConstPtr&)>)
boost::bind(&RosAriaNode::cmdvel_cb, this, _1 ));
// advertise enable/disable services
enable_srv = n.advertiseService("enable_motors", &RosAriaNode::enable_motors_cb, this);
disable_srv = n.advertiseService("disable_motors", &RosAriaNode::disable_motors_cb, this);
veltime = ros::Time::now();
sonar_tf_timer = n.createTimer(ros::Duration(0.033), &RosAriaNode::sonarCallback, this);
sonar_tf_timer.stop();
dynamic_reconfigure_server->setCallback(boost::bind(&RosAriaNode::dynamic_reconfigureCB, this, _1, _2));
// callback will be called by ArRobot background processing thread for every SIP data packet received from robot
robot->addSensorInterpTask("ROSPublishingTask", 100, &myPublishCB);
// Run ArRobot background processing thread
robot->runAsync(true);
return 0;
}
int main( int argc, char ** argv )
{
ros::init( argc, argv, "sonar_driver" );
if (argc < 3) {
ROS_ERROR("sonar_driver usage: sudo rosrun sonar_driver sonar_driver PORT BITRATE\n");
//port 0, bitrate 40000kHz
return 1;
}
// Initialize Cheetah parameters
port = atoi(argv[1]);
bitrate = atoi(argv[2]); //kHz
// Open the device
handle = ch_open(port);
if (handle <= 0) {
ROS_ERROR("Unable to open Cheetah device on port %d (Error code = %d: %s)", port, handle, ch_status_string(handle));
exit(1);
}
ROS_INFO("Opened Cheetah device on port %d", port);
ROS_INFO("Host interface is %s", (ch_host_ifce_speed(handle)) ? "high speed" : "full speed");
// Configure SPI subsystem
ch_spi_configure(handle, CH_SPI_POL_RISING_FALLING, CH_SPI_PHASE_SETUP_SAMPLE, CH_SPI_BITORDER_MSB, 0x0);
ROS_INFO("SPI configuration set to mode %d, %s shift, SS[2:0] active low", mode, "MSB");
// Power the target using the Cheetah's 5V power supply
ch_target_power(handle, CH_TARGET_POWER_ON);
ch_sleep_ms(100);
// Set the bitrate
bitrate = ch_spi_bitrate(handle, bitrate);
ROS_INFO("Bitrate set to %d kHz", bitrate);
// Make a simple queue to assert OE
ch_spi_queue_clear(handle);
ch_spi_queue_oe(handle, 1);
ch_spi_batch_shift(handle, 0, 0);
// Queue the batch
ROS_INFO("Beginning to queue SPI packets...");
ch_spi_queue_clear(handle);
for (int i = 0; i < DATA_BLOCK_SIZE; ++i) {
// Convert Slave 1
ch_spi_queue_ss(handle, 0xF);
ch_spi_queue_array(handle, 2, data_out);
ch_spi_queue_ss(handle, 0xE);
// Convert Slave 2
ch_spi_queue_ss(handle, 0xF);
ch_spi_queue_array(handle, 2, data_out);
ch_spi_queue_ss(handle, 0xD);
// Convert Slave 3
ch_spi_queue_ss(handle, 0xF);
ch_spi_queue_array(handle, 2, data_out);
ch_spi_queue_ss(handle, 0xB);
}
ROS_INFO("Finished queueing packets\n");
// Open output filestreams
std::ofstream file1 ("1_adc_samples.txt");
std::ofstream file2 ("2_adc_samples.txt");
std::ofstream file3 ("3_adc_samples.txt");
int batch_cnt = 1; //count number of sample batches
double elapsed;
s64 start;
while( ros::ok() ) {
// Submit the batch and collect data
ROS_INFO("Collecting data from batch %d...", batch_cnt);
start = _timeMillis();
ch_spi_async_submit(handle);
ret = ch_spi_async_collect(handle, TX_LENGTH, data_in);
elapsed = ((double)(_timeMillis() - start)) / 1000;
ROS_INFO("collected %d bytes from batch #%04d in %.4lf seconds\n", ret, batch_cnt, elapsed);
if (ret < 0) ROS_INFO("status error: %s\n", ch_status_string(ret));
batch_cnt++;
// Process raw data for the 12-bit ADC's, and write data to text files
int data_idx = 0;
if ( file1.is_open() && file2.is_open() && file3.is_open() ) {
for (int j = 0; j < TX_LENGTH; j += 6) {
// SS3 Data
input = (data_in[j] << 8) + data_in[j+1];
valid_data_point = (input & 0x3ffc) >> 2;
if(valid_data_point >= 0x0800) valid_data_point = valid_data_point - 0x1000; //convert 2's comp to signed
data3[data_idx] = valid_data_point;
file3 << data3[data_idx] << ",";
// SS1 Data
input = (data_in[j+2] << 8) + data_in[j+3];
valid_data_point = (input & 0x3ffc) >> 2;
if(valid_data_point >= 0x0800) valid_data_point = valid_data_point - 0x1000;
data1[data_idx] = valid_data_point;
file1 << data1[data_idx] << ",";
// SS2 Data
input = (data_in[j+4] << 8) + data_in[j+5];
valid_data_point = (input & 0x3ffc) >> 2;
if(valid_data_point >= 0x0800) valid_data_point = valid_data_point - 0x1000;
data2[data_idx] = valid_data_point;
file2 << data2[data_idx] << ",";
++data_idx;
}
}
else std::cout << "Error opening output filestream!" << std::endl;
}
bool Subscription::negotiateConnection(const std::string& xmlrpc_uri)
{
XmlRpcValue tcpros_array, protos_array, params;
XmlRpcValue udpros_array;
ros::TransportUDPPtr udp_transport;
int protos = 0;
ros::V_string transports = transport_hints_.getTransports();
if (transports.empty())
{
transport_hints_.reliable();
transports = transport_hints_.getTransports();
}
for (ros::V_string::const_iterator it = transports.begin();
it != transports.end();
++it)
{
if (*it == "UDP")
{
int max_datagram_size = transport_hints_.getMaxDatagramSize();
udp_transport = ros::TransportUDPPtr(new ros::TransportUDP(&ros::PollManager::instance()->getPollSet()));
if (!max_datagram_size)
max_datagram_size = udp_transport->getMaxDatagramSize();
udp_transport->createIncoming(0, false);
udpros_array[0] = "UDPROS";
M_string m;
m["topic"] = getName();
m["md5sum"] = md5sum();
m["callerid"] = ros::this_node::getName();
m["type"] = datatype();
boost::shared_array<uint8_t> buffer;
uint32_t len;
ros::Header::write(m, buffer, len);
XmlRpcValue v(buffer.get(), len);
udpros_array[1] = v;
udpros_array[2] = ros::network::getHost();
udpros_array[3] = udp_transport->getServerPort();
udpros_array[4] = max_datagram_size;
protos_array[protos++] = udpros_array;
}
else if (*it == "TCP")
{
tcpros_array[0] = std::string("TCPROS");
protos_array[protos++] = tcpros_array;
}
else
{
ROS_WARN("Unsupported transport type hinted: %s, skipping", it->c_str());
}
}
params[0] = ros::this_node::getName();
params[1] = name_;
params[2] = protos_array;
std::string peer_host;
uint32_t peer_port;
if (!ros::network::splitURI(xmlrpc_uri, peer_host, peer_port))
{
ROS_ERROR("Bad xml-rpc URI: [%s]", xmlrpc_uri.c_str());
return false;
}
XmlRpcClient* c = new XmlRpcClient(peer_host.c_str(),
peer_port, "/");
// if (!c.execute("requestTopic", params, result) || !g_node->validateXmlrpcResponse("requestTopic", result, proto))
// Initiate the negotiation. We'll come back and check on it later.
if (!c->executeNonBlock("requestTopic", params))
{
ROSCPP_LOG_DEBUG("Failed to contact publisher [%s:%d] for topic [%s]",
peer_host.c_str(), peer_port, name_.c_str());
delete c;
if (udp_transport)
{
udp_transport->close();
}
return false;
}
ROSCPP_LOG_DEBUG("Began asynchronous xmlrpc connection to [%s:%d]", peer_host.c_str(), peer_port);
// The PendingConnectionPtr takes ownership of c, and will delete it on
// destruction.
PendingConnectionPtr conn(new PendingConnection(c, udp_transport, shared_from_this(), xmlrpc_uri));
//ROS_INFO("add connection to xmlrpcmanager");
XMLRPCManager::instance()->addASyncConnection(conn);
// Put this connection on the list that we'll look at later.
{
boost::mutex::scoped_lock pending_connections_lock(pending_connections_mutex_);
pending_connections_.insert(conn);
}
return true;
}
bool LCM2ROSControl::initRequest(hardware_interface::RobotHW* robot_hw,
ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh,
std::set<std::string>& claimed_resources)
{
// check if construction finished cleanly
if (state_ != CONSTRUCTED){
ROS_ERROR("Cannot initialize this controller because it failed to be constructed");
return false;
}
// setup LCM (todo: move to constructor? how to propagate an error then?)
lcm_ = boost::shared_ptr<lcm::LCM>(new lcm::LCM);
if (!lcm_->good())
{
std::cerr << "ERROR: lcm is not good()" << std::endl;
return false;
}
handler_ = std::shared_ptr<LCM2ROSControl_LCMHandler>(new LCM2ROSControl_LCMHandler(*this));
// get a pointer to the effort interface
hardware_interface::EffortJointInterface* effort_hw = robot_hw->get<hardware_interface::EffortJointInterface>();
if (!effort_hw)
{
ROS_ERROR("This controller requires a hardware interface of type hardware_interface::EffortJointInterface.");
return false;
}
effort_hw->clearClaims();
const std::vector<std::string>& effortNames = effort_hw->getNames();
// initialize command buffer for each joint we found on the HW
for(unsigned int i=0; i<effortNames.size(); i++)
{
effortJointHandles[effortNames[i]] = effort_hw->getHandle(effortNames[i]);
latest_commands[effortNames[i]] = drc::joint_command_t();
latest_commands[effortNames[i]].joint_name = effortNames[i];
latest_commands[effortNames[i]].position = 0.0;
latest_commands[effortNames[i]].velocity = 0.0;
latest_commands[effortNames[i]].effort = 0.0;
latest_commands[effortNames[i]].k_q_p = 0.0;
latest_commands[effortNames[i]].k_q_i = 0.0;
latest_commands[effortNames[i]].k_qd_p = 0.0;
latest_commands[effortNames[i]].k_f_p = 0.0;
latest_commands[effortNames[i]].ff_qd = 0.0;
latest_commands[effortNames[i]].ff_qd_d = 0.0;
latest_commands[effortNames[i]].ff_f_d = 0.0;
latest_commands[effortNames[i]].ff_const = 0.0;
}
auto effort_hw_claims = effort_hw->getClaims();
claimed_resources.insert(effort_hw_claims.begin(), effort_hw_claims.end());
effort_hw->clearClaims();
// get a pointer to the imu interface
hardware_interface::ImuSensorInterface* imu_hw = robot_hw->get<hardware_interface::ImuSensorInterface>();
if (!imu_hw)
{
ROS_ERROR("This controller requires a hardware interface of type hardware_interface::ImuSensorInterface.");
return false;
}
imu_hw->clearClaims();
const std::vector<std::string>& imuNames = imu_hw->getNames();
for(unsigned int i=0; i<imuNames.size(); i++)
{
imuSensorHandles[imuNames[i]] = imu_hw->getHandle(imuNames[i]);
}
auto imu_hw_claims = imu_hw->getClaims();
claimed_resources.insert(imu_hw_claims.begin(), imu_hw_claims.end());
imu_hw->clearClaims();
hardware_interface::ForceTorqueSensorInterface* forceTorque_hw = robot_hw->get<hardware_interface::ForceTorqueSensorInterface>();
if (!forceTorque_hw)
{
ROS_ERROR("This controller requires a hardware interface of type hardware_interface::EffortJointInterface.");
return false;
}
// get pointer to forcetorque interface
forceTorque_hw->clearClaims();
const std::vector<std::string>& forceTorqueNames = forceTorque_hw->getNames();
for(unsigned int i=0; i<forceTorqueNames.size(); i++)
{
forceTorqueHandles[forceTorqueNames[i]] = forceTorque_hw->getHandle(forceTorqueNames[i]);
}
auto forceTorque_hw_claims = forceTorque_hw->getClaims();
claimed_resources.insert(forceTorque_hw_claims.begin(), forceTorque_hw_claims.end());
forceTorque_hw->clearClaims();
// success
state_ = INITIALIZED;
ROS_INFO("LCM2ROSCONTROL ON\n");
return true;
}
bool SerialInterface::getPacket (char *spacket, unsigned char &packet_type, unsigned short &packet_crc,
unsigned short &packet_size)
{
char stoken[4];
char ssize[2];
char stype[1];
char scrc[2];
int bytes;
int i;
ROS_DEBUG (" SerialInterface::getPacket()");
// get beginning (">*>")
stoken[0] = '\0';
stoken[1] = '\0';
stoken[2] = '\0';
stoken[3] = '\0';
wait(3);
i = read (dev_,stoken, 3);
if (i == 0 || strncmp (stoken, ">*>", 3) != 0)
{
ROS_DEBUG (" dev: %zd", (size_t) dev_);
ROS_ERROR (" Error Reading Packet Header: %s", strerror (errno));
ROS_ERROR (" Read (%d): %s", i, stoken);
//ROS_BREAK();
//while (read (dev_,stoken, 1) != 0) {}
flush ();
return false;
}
serialport_bytes_rx_ += 3;
ROS_DEBUG (" Packet Header OK");
// get packet size
wait(2);
i = read (dev_,ssize, 2);
if (i == 0)
{
ROS_ERROR ("Error Reading Packet Size: %s", strerror (errno));
flush ();
return false;
}
serialport_bytes_rx_ += 2;
memcpy (&packet_size, ssize, sizeof (packet_size));
//ROS_DEBUG ("Packet size: %d", packet_size);
// get packet type
wait(1);
i = read (dev_, stype, 1);
if (i == 0)
return false;
serialport_bytes_rx_ += 1;
memcpy (&packet_type, stype, sizeof (packet_type));
//ROS_DEBUG ("Packet type: %d", packet_type);
// get packet
wait(packet_size);
i = read (dev_, spacket, packet_size);
if (i == 0)
return false;
serialport_bytes_rx_ += packet_size;
//ROS_DEBUG ("Packet string: ok");
// get packet crc
wait(2);
i = read (dev_, scrc, 2);
if (i == 0)
return false;
serialport_bytes_rx_ += sizeof (scrc);
memcpy (&packet_crc, scrc, sizeof (packet_crc));
//ROS_DEBUG ("Packet crc: %d", packet_crc);
// get closing ("<#<")
wait(3);
i = read (dev_, stoken, 3);
if (i == 0 || strncmp (stoken, "<#<", 3) != 0)
{
ROS_ERROR ("Error Reading Packet Footer: %s", strerror (errno));
ROS_DEBUG ("Read (%d): %s", i, stoken);
while (read (dev_, stoken, 1) != 0)
{
stoken[1] = '\0';
ROS_DEBUG ("%s", stoken);
}
flush ();
drain ();
ROS_DEBUG ("Packet Footer Corrupt!!");
return false;
}
serialport_bytes_rx_ += 3;
//ROS_DEBUG ("Packet Footer OK");
return true;
}
int main(int argc, char **argv)
{
char** _argv = NULL;
int _argc = 0;
int Narg=0;
int cuentaObs=0;
float Lx=0.0, Ly=0.0;
float periodo, f;
int k=0;
std::string fileName;
Narg=6;
if (argc>Narg)
{
Ly = atof(argv[1]);
Lx = atof(argv[2]);
nCeldas_i = atoi(argv[3]);
nCeldas_j = atoi(argv[4]);
fileName = argv[5];
cantidadObstaculos = atoi(argv[6]);
} else{
ROS_ERROR("Nodo 5: Indique argumentos completos!\n");
return (0);
}
//--- Inicializacion de nodo de ROS
ros::init(_argc,_argv,"Nodo5_ConstruccionDeMapa");
ROS_INFO("Nodo5_ConstruccionDeMapa just started");
ros::NodeHandle node;
//-- Topicos susbcritos y publicados
ros::Subscriber subInfo1=node.subscribe("/vrep/info",100,infoCallback);
ros::Subscriber subInfo2=node.subscribe("UbicacionRobot",100,ubicacionRobCallback);
ros::Subscriber subInfo3=node.subscribe("Plan",100,ajusteCallback);
chatter_pub = node.advertise<camina::InfoMapa>("GraficaMapa", 100);
//Clientes y Servicios
// client_EspacioTrabajo=node.serviceClient<camina::EspacioTrabajoParametros>("EspacioTrabajo");
//--- Inicializacion de variables
LongitudCeldaY = infoMapa.tamanoCelda_i = Ly/(float) nCeldas_i;
LongitudCeldaX = infoMapa.tamanoCelda_j = Lx/(float) nCeldas_j;
// infoMapa.tamanoCelda_i = Ly/(float) nCeldas_i;
// infoMapa.tamanoCelda_j = Lx/(float) nCeldas_j;
//-- Inicializacion de mensaje para graficar
infoMapa.nCeldas_i=nCeldas_i;
infoMapa.nCeldas_j=nCeldas_j;
infoMapa.cantidadObstaculos = cantidadObstaculos;
for (k=0; k<cantidadObstaculos;k++) {
infoMapa.coordenadaObstaculo_i.push_back(0);
infoMapa.coordenadaObstaculo_j.push_back(0);
}
for (k=0; k<Npatas;k++) {
infoMapa.coordenadaPata_i.push_back(0);
infoMapa.coordenadaPata_j.push_back(0);
infoMapa.pataApoyo.push_back(0);
}
// Inicializacion de apuntadores
p_ij = ij;
// Inicializacion de matriz nula
for(int i=0;i<nCeldas_i;i++){
for(int j=0;j<nCeldas_j;j++){
matrizMapa[i][j]=-1;
bool_matrizMapa[i][j]=false;
}
}
cuentaObs = Construye_matrizMapa(fileName, nCeldas_i, nCeldas_j);
printf("obstaculos recibidos: %d, obstaculos contados: %d\n",cantidadObstaculos,cuentaObs);
//--- Inicializacion de grafica
// IniciaGrafica();
//--- Inicializacion de archivo de salida
fp1 = fopen("../fuerte_workspace/sandbox/TesisMaureen/ROS/camina/datos/SalidaMapa_robot.txt","w+");
fp2 = fopen("../fuerte_workspace/sandbox/TesisMaureen/ROS/camina/datos/SalidaMapa_ajuste.txt","w+");
//--- Ciclo de ROS
// periodo=1.5;
// f=1/periodo;
// ros::Rate loop_rate(f); //Frecuencia [Hz]
// for (int i=0; i<3; i++) loop_rate.sleep();
chatter_pub.publish(infoMapa);
while (ros::ok() && simulationRunning)
{
// for (int k=0; k<Npatas;k++) {
// //-- Punto 1
// transformacion_yxTOij(p_ij,EspacioTrabajoPata_y[k][0],EspacioTrabajoPata_x[k][0]);
// EspacioTrabajoPatas_ij[k][0]=ij[0];
// EspacioTrabajoPatas_ij[k][1]=ij[1];
// //-- Punto 2
// transformacion_yxTOij(p_ij,EspacioTrabajoPata_y[k][1],EspacioTrabajoPata_x[k][1]);
// EspacioTrabajoPatas_ij[k][2]=ij[0];
// EspacioTrabajoPatas_ij[k][3]=ij[1];
// //-- Punto 3
// transformacion_yxTOij(p_ij,EspacioTrabajoPata_y[k][2],EspacioTrabajoPata_x[k][2]);
// EspacioTrabajoPatas_ij[k][4]=ij[0];
// EspacioTrabajoPatas_ij[k][5]=ij[1];
// //-- Punto 4
// transformacion_yxTOij(p_ij,EspacioTrabajoPata_y[k][3],EspacioTrabajoPata_x[k][3]);
// EspacioTrabajoPatas_ij[k][6]=ij[0];
// EspacioTrabajoPatas_ij[k][7]=ij[1];
// }
// //-- Funcion para grafica de cuerpo
// FuncionGrafica_Cuerpo();
// chatter_pub.publish(infoMapa);
ros::spinOnce();
// loop_rate.sleep();
}
fclose(fp1); fclose(fp2);
ROS_INFO("AdiosGrafica!");
ros::shutdown();
return 0;
}
bool SerialInterface::getPackets (Telemetry * telemetry)
{
flush ();
ROS_DEBUG ("SerialInterface::getPackets");
char cmd[16];
char spacket[1024];
unsigned char packet_type;
unsigned short packet_crc;
unsigned short packet_size;
unsigned int i;
bool result = false;
ros::Time packetTime;
ROS_DEBUG (" Requesting %04x %zd packets", (short) telemetry->requestPackets_.to_ulong (),
telemetry->requestPackets_.count ());
sprintf (cmd, ">*>p%c", (short) telemetry->requestPackets_.to_ulong ());
output (cmd, 6);
for (i = 0; i < telemetry->requestPackets_.count (); i++)
{
packetTime = ros::Time::now(); // Presumes that the AutoPilot is grabbing the data for each packet
// immediately prior to each packet being sent, as opposed to gathering
// all the data at once and then sending it all. Either way is a guess
// unless we get some info from AscTec one way or the other..
bool read_result = getPacket (spacket, packet_type, packet_crc, packet_size);
if (read_result)
{
ROS_DEBUG (" Read successful: type = %d, crc = %d", packet_type, packet_crc);
if (packet_type == Telemetry::PD_LLSTATUS)
{
ROS_DEBUG (" Packet type is LL_STATUS");
memcpy (&telemetry->LL_STATUS_, spacket, packet_size);
telemetry->timestamps_[RequestTypes::LL_STATUS] = packetTime;
if (crc_valid (packet_crc, &telemetry->LL_STATUS_, sizeof (packet_size)))
{
result = true;
}
//telemetry->dumpLL_STATUS();
}
else if (packet_type == Telemetry::PD_IMURAWDATA)
{
ROS_DEBUG (" Packet type is IMU_RAWDATA");
memcpy (&telemetry->IMU_RAWDATA_, spacket, packet_size);
telemetry->timestamps_[RequestTypes::IMU_RAWDATA] = packetTime;
if (crc_valid (packet_crc, &telemetry->IMU_RAWDATA_, packet_size))
{
result = true;
}
//telemetry->dumpIMU_RAWDATA();
}
else if (packet_type == Telemetry::PD_IMUCALCDATA)
{
ROS_DEBUG (" Packet type is IMU_CALCDATA");
memcpy (&telemetry->IMU_CALCDATA_, spacket, packet_size);
telemetry->timestamps_[RequestTypes::IMU_CALCDATA] = packetTime;
if (crc_valid (packet_crc, &telemetry->IMU_CALCDATA_, packet_size))
{
result = true;
}
//telemetry->dumpIMU_CALCDATA();
}
else if (packet_type == Telemetry::PD_RCDATA)
{
ROS_DEBUG (" Packet type is RC_DATA");
memcpy (&telemetry->RC_DATA_, spacket, packet_size);
telemetry->timestamps_[RequestTypes::RC_DATA] = packetTime;
if (crc_valid (packet_crc, &telemetry->RC_DATA_, packet_size))
{
result = true;
}
//telemetry->dumpRC_DATA();
}
else if (packet_type == Telemetry::PD_CTRLOUT)
{
ROS_DEBUG (" Packet type is CONTROLLER_OUTPUT");
memcpy (&telemetry->CONTROLLER_OUTPUT_, spacket, packet_size);
telemetry->timestamps_[RequestTypes::CONTROLLER_OUTPUT] = packetTime;
if (crc_valid (packet_crc, &telemetry->CONTROLLER_OUTPUT_, packet_size))
{
result = true;
}
//telemetry->dumpCONTROLLER_OUTPUT();
}
else if (packet_type == Telemetry::PD_GPSDATA)
{
ROS_DEBUG (" Packet type is GPS_DATA");
memcpy (&telemetry->GPS_DATA_, spacket, packet_size);
telemetry->timestamps_[RequestTypes::GPS_DATA] = packetTime;
if (crc_valid (packet_crc, &telemetry->GPS_DATA_, packet_size))
{
result = true;
}
//telemetry->dumpGPS_DATA();
}
else if (packet_type == Telemetry::PD_GPSDATAADVANCED)
{
ROS_DEBUG (" Packet type is GPS_DATA_ADVANCED");
memcpy (&telemetry->GPS_DATA_ADVANCED_, spacket, packet_size);
telemetry->timestamps_[RequestTypes::GPS_DATA_ADVANCED] = packetTime;
if (crc_valid (packet_crc, &telemetry->GPS_DATA_ADVANCED_, packet_size))
{
result = true;
}
//telemetry->dumpGPS_DATA_ADVANCED();
}
else
{
ROS_ERROR (" Packet type (%#2x) is UNKNOWN", packet_type);
}
}
else
{
// failed read
ROS_ERROR (" Read failed");
break;
}
}
ioctl(dev_,FIONREAD,&i);
if (i != 0)
{
ROS_ERROR (" Unexpected Data: Flushing receive buffer");
flush();
}
return result;
}
FootstepNavigation::FootstepNavigation()
: ivIdFootRight("/r_sole"),
ivIdFootLeft("/l_sole"),
ivIdMapFrame("map"),
ivExecutingFootsteps(false),
ivFootstepsExecution("footsteps_execution", true),
ivExecutionShift(2),
ivControlStepIdx(-1),
ivResetStepIdx(0)
{
// private NodeHandle for parameters and private messages (debug / info)
ros::NodeHandle nh_private("~");
ros::NodeHandle nh_public;
// service
ivFootstepSrv =
nh_public.serviceClient<humanoid_nav_msgs::StepTargetService>(
"footstep_srv");
ivClipFootstepSrv =
nh_public.serviceClient<humanoid_nav_msgs::ClipFootstep>(
"clip_footstep_srv");
// subscribers
ivGridMapSub =
nh_public.subscribe<nav_msgs::OccupancyGrid>(
"map", 1, &FootstepNavigation::mapCallback, this);
ivGoalPoseSub =
nh_public.subscribe<geometry_msgs::PoseStamped>(
"goal", 1, &FootstepNavigation::goalPoseCallback, this);
// read parameters from config file:
nh_private.param("rfoot_frame_id", ivIdFootRight, ivIdFootRight);
nh_private.param("lfoot_frame_id", ivIdFootLeft, ivIdFootLeft);
nh_private.param("accuracy/footstep/x", ivAccuracyX, 0.01);
nh_private.param("accuracy/footstep/y", ivAccuracyY, 0.01);
nh_private.param("accuracy/footstep/theta", ivAccuracyTheta, 0.1);
nh_private.param("accuracy/cell_size", ivCellSize, 0.005);
nh_private.param("accuracy/num_angle_bins", ivNumAngleBins, 128);
nh_private.param("forward_search", ivForwardSearch, false);
nh_private.param("feedback_frequency", ivFeedbackFrequency, 5.0);
nh_private.param("safe_execution", ivSafeExecution, true);
nh_private.param("foot/max/step/x", ivMaxStepX, 0.07);
nh_private.param("foot/max/step/y", ivMaxStepY, 0.15);
nh_private.param("foot/max/step/theta", ivMaxStepTheta, 0.3);
nh_private.param("foot/max/inverse/step/x", ivMaxInvStepX, -0.03);
nh_private.param("foot/max/inverse/step/y", ivMaxInvStepY, 0.09);
nh_private.param("foot/max/inverse/step/theta", ivMaxInvStepTheta, -0.01);
// step range
XmlRpc::XmlRpcValue step_range_x;
XmlRpc::XmlRpcValue step_range_y;
nh_private.getParam("step_range/x", step_range_x);
nh_private.getParam("step_range/y", step_range_y);
if (step_range_x.getType() != XmlRpc::XmlRpcValue::TypeArray)
ROS_ERROR("Error reading footsteps/x from config file.");
if (step_range_y.getType() != XmlRpc::XmlRpcValue::TypeArray)
ROS_ERROR("Error reading footsteps/y from config file.");
if (step_range_x.size() != step_range_y.size())
{
ROS_ERROR("Step range points have different size. Exit!");
exit(2);
}
// create step range
ivStepRange.clear();
ivStepRange.reserve(step_range_x.size());
double x, y;
for (int i = 0; i < step_range_x.size(); ++i)
{
x = (double)step_range_x[i];
y = (double)step_range_y[i];
ivStepRange.push_back(std::pair<double, double>(x, y));
}
// insert first point again at the end!
ivStepRange.push_back(ivStepRange[0]);
}
/*!
* \brief Gets parameters from the ROS parameter server and configures the powercube_chain.
*/
void getROSParameters()
{
// get CanModule
std::string CanModule;
if (n_.hasParam("can_module"))
{
n_.getParam("can_module", CanModule);
}
else
{
ROS_ERROR("Parameter can_module not set, shutting down node...");
n_.shutdown();
}
// get CanDevice
std::string CanDevice;
if (n_.hasParam("can_device"))
{
n_.getParam("can_device", CanDevice);
}
else
{
ROS_ERROR("Parameter can_device not set, shutting down node...");
n_.shutdown();
}
// get CanBaudrate
int CanBaudrate;
if (n_.hasParam("can_baudrate"))
{
n_.getParam("can_baudrate", CanBaudrate);
}
else
{
ROS_ERROR("Parameter can_baudrate not set, shutting down node...");
n_.shutdown();
}
// get modul ids
XmlRpc::XmlRpcValue ModulIDsXmlRpc;
std::vector<int> ModulIDs;
if (n_.hasParam("modul_ids"))
{
n_.getParam("modul_ids", ModulIDsXmlRpc);
}
else
{
ROS_ERROR("Parameter modul_ids not set, shutting down node...");
n_.shutdown();
}
ModulIDs.resize(ModulIDsXmlRpc.size());
for (int i = 0; i < ModulIDsXmlRpc.size(); i++)
{
ModulIDs[i] = (int)ModulIDsXmlRpc[i];
}
// init parameters
pc_params_->Init(CanModule, CanDevice, CanBaudrate, ModulIDs);
// get joint names
XmlRpc::XmlRpcValue JointNamesXmlRpc;
std::vector<std::string> JointNames;
if (n_.hasParam("joint_names"))
{
n_.getParam("joint_names", JointNamesXmlRpc);
}
else
{
ROS_ERROR("Parameter joint_names not set, shutting down node...");
n_.shutdown();
}
JointNames.resize(JointNamesXmlRpc.size());
for (int i = 0; i < JointNamesXmlRpc.size(); i++)
{
JointNames[i] = (std::string)JointNamesXmlRpc[i];
}
// check dimension with with DOF
if ((int)JointNames.size() != pc_params_->GetDOF())
{
ROS_ERROR("Wrong dimensions of parameter joint_names, shutting down node...");
n_.shutdown();
}
pc_params_->SetJointNames(JointNames);
// get max accelerations
XmlRpc::XmlRpcValue MaxAccelerationsXmlRpc;
std::vector<double> MaxAccelerations;
if (n_.hasParam("max_accelerations"))
{
n_.getParam("max_accelerations", MaxAccelerationsXmlRpc);
}
else
{
ROS_ERROR("Parameter max_accelerations not set, shutting down node...");
n_.shutdown();
}
MaxAccelerations.resize(MaxAccelerationsXmlRpc.size());
for (int i = 0; i < MaxAccelerationsXmlRpc.size(); i++)
{
MaxAccelerations[i] = (double)MaxAccelerationsXmlRpc[i];
}
// check dimension with with DOF
if ((int)MaxAccelerations.size() != pc_params_->GetDOF())
{
ROS_ERROR("Wrong dimensions of parameter max_accelerations, shutting down node...");
n_.shutdown();
}
pc_params_->SetMaxAcc(MaxAccelerations);
// get horizon
double Horizon;
if (n_.hasParam("horizon"))
{
n_.getParam("horizon", Horizon);
}
else
{
Horizon = 0.025; //Hz
ROS_WARN("Parameter horizon not available, setting to default value: %f sec", Horizon);
}
pc_ctrl_->setHorizon(Horizon);
}
void GazeboRosControllerManager::LoadThread()
{
// Get then name of the parent model
std::string modelName = this->sdf->GetParent()->GetValueString("name");
// Get the world name.
this->world = this->parent_model_->GetWorld();
// Error message if the model couldn't be found
if (!this->parent_model_)
ROS_ERROR("parent model in NULL.");
if (getenv("CHECK_SPEEDUP"))
{
wall_start_ = this->world->GetRealTime().Double();
sim_start_ = this->world->GetSimTime().Double();
}
// check update rate against world physics update rate
// should be equal or higher to guarantee the wrench applied is not "diluted"
// if (this->updatePeriod > 0 &&
// (this->world->GetPhysicsEngine()->GetUpdateRate() >
// 1.0/this->updatePeriod))
// ROS_ERROR("gazebo_ros_controller_manager update rate is less than"
// " physics update rate, wrench applied will be diluted"
// " (applied intermittently)");
// get parameter name
this->robotNamespace = "";
if (this->sdf->HasElement("robotNamespace"))
this->robotNamespace =
this->sdf->GetElement("robotNamespace")->GetValueString();
this->robotParam = "robot_description";
if (this->sdf->HasElement("robotParam"))
this->robotParam =
this->sdf->GetElement("robotParam")->GetValueString();
this->robotParam = this->robotNamespace+"/" + this->robotParam;
// Exit if no ROS
if (!ros::isInitialized())
{
gzerr << "Not loading plugin since ROS hasn't been "
<< "properly initialized. Try starting gazebo with ros plugin:\n"
<< " gazebo -s libgazebo_ros_api_plugin.so\n";
return;
}
/* Init ROS
if (!ros::isInitialized())
{
int argc = 0;
char** argv = NULL;
ros::init( argc, argv, "gazebo", ros::init_options::NoSigintHandler);
gzwarn << "should start ros::init in simulation by using the"
<< " system plugin\n";
}*/
this->rosnode_ = new ros::NodeHandle(this->robotNamespace);
ROS_INFO("starting gazebo_ros_controller_manager plugin in ns: %s",
this->robotNamespace.c_str());
// pr2_etherCAT calls ros::spin(), we'll thread out one spinner
// here to mimic that
this->ros_spinner_thread_ = boost::thread(
boost::bind(&GazeboRosControllerManager::ControllerManagerROSThread, this));
// load a controller manager, initialize hardware_interface
this->controller_manager_ = new pr2_controller_manager::ControllerManager(
&hardware_interface_, *this->rosnode_);
this->hardware_interface_.current_time_ = ros::Time(
this->world->GetSimTime().Double());
// hardcoded to minimum of 1ms on start up
if (this->hardware_interface_.current_time_ < ros::Time(0.001))
this->hardware_interface_.current_time_ == ros::Time(0.001);
this->rosnode_->param("gazebo/start_robot_calibrated",
this->calibration_status_, true);
// Read urdf from ros parameter server then
// setup actuators and mechanism control node.
// This call will block if ROS is not properly initialized.
if (!LoadControllerManagerFromURDF())
{
ROS_ERROR("Error parsing URDF in gazebo controller manager plugin,"
" plugin not active.\n");
return;
}
// Initializes the fake state (for running the transmissions backwards).
this->virtual_mechanism_state_ =
new pr2_mechanism_model::RobotState(&this->controller_manager_->model_);
// The gazebo joints and mechanism joints should match up.
for (unsigned int i = 0;
i < this->controller_manager_->state_->joint_states_.size(); ++i)
{
std::string joint_name =
this->controller_manager_->state_->joint_states_[i].joint_->name;
// fill in gazebo joints pointer
gazebo::physics::JointPtr joint =
this->parent_model_->GetJoint(this->parent_model_->GetName() +
"::" + joint_name);
if (joint)
{
this->gazebo_joints_.push_back(joint);
}
else
{
ROS_WARN("A Mechanism Controlled joint named [%s] is not found"
" in gazebo model[%s].\n",
joint_name.c_str(), this->parent_model_->GetName().c_str());
this->gazebo_joints_.push_back(gazebo::physics::JointPtr());
}
}
assert(this->gazebo_joints_.size() ==
this->virtual_mechanism_state_->joint_states_.size());
#ifdef USE_CBQ
// start custom queue for controller manager
this->controller_manager_callback_queue_thread_ = boost::thread(boost::bind(
&GazeboRosControllerManager::ControllerManagerQueueThread, this));
#endif
// If all previous steps are successful, start the controller manager
// plugin updates
this->updateConnection = event::Events::ConnectWorldUpdateBegin(
boost::bind(&GazeboRosControllerManager::UpdateControllerForces, this));
}
/*!
* \brief Gets parameters from the robot_description and configures the powercube_chain.
*/
void getRobotDescriptionParameters()
{
unsigned int DOF = pc_params_->GetDOF();
std::vector<std::string> JointNames = pc_params_->GetJointNames();
// get robot_description from ROS parameter server
std::string param_name = "robot_description";
std::string full_param_name;
std::string xml_string;
n_.searchParam(param_name, full_param_name);
if (n_.hasParam(full_param_name))
{
n_.getParam(full_param_name.c_str(), xml_string);
}
else
{
ROS_ERROR("Parameter %s not set, shutting down node...", full_param_name.c_str());
n_.shutdown();
}
if (xml_string.size() == 0)
{
ROS_ERROR("Unable to load robot model from parameter %s",full_param_name.c_str());
n_.shutdown();
}
ROS_DEBUG("%s content\n%s", full_param_name.c_str(), xml_string.c_str());
// get urdf model out of robot_description
urdf::Model model;
if (!model.initString(xml_string))
{
ROS_ERROR("Failed to parse urdf file");
n_.shutdown();
}
ROS_DEBUG("Successfully parsed urdf file");
// get max velocities out of urdf model
std::vector<double> MaxVelocities(DOF);
for (unsigned int i = 0; i < DOF; i++)
{
MaxVelocities[i] = model.getJoint(JointNames[i].c_str())->limits->velocity;
}
// get lower limits out of urdf model
std::vector<double> LowerLimits(DOF);
for (unsigned int i = 0; i < DOF; i++)
{
LowerLimits[i] = model.getJoint(JointNames[i].c_str())->limits->lower;
}
// get upper limits out of urdf model
std::vector<double> UpperLimits(DOF);
for (unsigned int i = 0; i < DOF; i++)
{
UpperLimits[i] = model.getJoint(JointNames[i].c_str())->limits->upper;
}
// get offsets out of urdf model
std::vector<double> Offsets(DOF);
for (unsigned int i = 0; i < DOF; i++)
{
Offsets[i] = model.getJoint(JointNames[i].c_str())->calibration->rising.get()[0];
}
// set parameters
pc_params_->SetMaxVel(MaxVelocities);
pc_params_->SetLowerLimits(LowerLimits);
pc_params_->SetUpperLimits(UpperLimits);
pc_params_->SetOffsets(Offsets);
}
void HermesMoveArmActionServer::moveArm(const hermes_move_arm_action::MoveArmGoalConstPtr& goal)
{
// this callback function is executed each time a request (= goal message) comes in for this service server
ROS_INFO("MoveArm Action Server: Received a request for arm %i.", goal->arm);
if (goal->goal_position.header.frame_id.compare("/world") != 0)
{
ROS_ERROR("The goal position coordinates are not provided in the correct frame. The required frame is '/world' but '%s' was provided.", goal->goal_position.header.frame_id.c_str());
return;
}
// this command sends a feedback message to the caller, here we transfer that the task is completed 25%
// hermes_move_arm_action::MoveArmFeedback feedback;
// feedback.percentageDone = 25;
// move_arm_action_server_.publishFeedback(feedback);
// move the arm
// ...
//goal->goal_position.pose.position.x //(x,y,z) in meters?????
//goal->goal_position.pose.orientation.w //(w,x,y,z) Quaternion in rad
//goal->goal_position.header.frame_id // Reference frame
tf::Quaternion quaternion;
tf::quaternionMsgToTF(goal->goal_position.pose.orientation, quaternion);
tf::Transform goalPos(quaternion, tf::Vector3(goal->goal_position.pose.position.x,goal->goal_position.pose.position.y,goal->goal_position.pose.position.z));
// Transform goalPos to Robot Reference frame
hermes_reference_frames_service::HermesFrame::Request req_frames;
hermes_reference_frames_service::HermesFrame::Response res_frames;
if(goal->arm == hermes_move_arm_action::MoveArmGoal::LEFTARM) // Depends of arm
req_frames.frame = hermes_reference_frames_service::HermesFrame::Request::WORLDTLEFTARM;
else if(goal->arm == hermes_move_arm_action::MoveArmGoal::RIGHTARM) // Depends of arm
req_frames.frame = hermes_reference_frames_service::HermesFrame::Request::WORLDTRIGHTARM;
ros::service::call("reference_frames_service", req_frames, res_frames);
// Transform world to base robot
tf::Quaternion qua_wTr(res_frames.quaternion[0],res_frames.quaternion[1],res_frames.quaternion[2],res_frames.quaternion[3]);
tf::Transform wTr(qua_wTr, tf::Vector3(res_frames.position[0],res_frames.position[1],res_frames.position[2]));
tf::Transform rTobj;
rTobj = wTr.inverse()*goalPos;
std::cout << "rTobj: " << std::endl;
for (int i=0; i<3; ++i)
{
std::cout << rTobj.getBasis()[i].getX() << "\t";
std::cout << rTobj.getBasis()[i].getY() << "\t";
std::cout << rTobj.getBasis()[i].getZ() <<std::endl;
}
std::cout << "XYZ: " << std::endl;
std::cout << rTobj.getOrigin().getX() <<std::endl;
std::cout << rTobj.getOrigin().getY() <<std::endl;
std::cout << rTobj.getOrigin().getZ() <<std::endl;
// tf::Vector3 rotX=rTobj.getBasis()[0];
// tf::Vector3 rotY=rTobj.getBasis()[1];
// tf::Vector3 rotZ=rTobj.getBasis()[2];
//
//
// KDL::Vector rotXkdl(rotX.getX(),rotX.getY(),rotX.getZ());
// KDL::Vector rotYkdl(rotY.getX(),rotY.getY(),rotY.getZ());
// KDL::Vector rotZkdl(rotZ.getX(),rotZ.getY(),rotZ.getZ());
//
// KDL::Rotation rot(rotXkdl,rotYkdl,rotZkdl);
//Get init position
std::vector<float> q_init;
if(goal->arm == hermes_move_arm_action::MoveArmGoal::LEFTARM)
hermesinterface.get_leftJoints(q_init);
else if(goal->arm == hermes_move_arm_action::MoveArmGoal::RIGHTARM)
hermesinterface.get_rightJoints(q_init);
// todo: call hermes_arm_kdl ikine service
hermes_arm_kdl::ikine::Request req_kdl;
hermes_arm_kdl::ikine::Response res_kdl;
for (int i=0; i<7; i++)
req_kdl.jointAngles_init[i] = q_init[i];
req_kdl.position[0] = rTobj.getOrigin().getX();
req_kdl.position[1] = rTobj.getOrigin().getY();
req_kdl.position[2] = rTobj.getOrigin().getZ();
for (int i=0; i<3; i++)
{
req_kdl.rotation[3*i] = rTobj.getBasis()[i].getX();
req_kdl.rotation[3*i+1] = rTobj.getBasis()[i].getY();
req_kdl.rotation[3*i+2] = rTobj.getBasis()[i].getZ();
}
ros::service::call("arm_kdl_service_ikine_server", req_kdl, res_kdl);
// Move the arm with res_kdl
std::vector<float> jointAngles(7);
for (int i=0; i<7; i++)
jointAngles[i] = res_kdl.jointAngles[i];
std::cout << "jointAngles:" << std::endl;
for (int i=0; i<7; i++)
std::cout << jointAngles[i] << std::endl;
//ARMS DON'T MOVE
if(goal->arm == hermes_move_arm_action::MoveArmGoal::LEFTARM)
hermesinterface.moveLeftArm(jointAngles);
else if(goal->arm == hermes_move_arm_action::MoveArmGoal::RIGHTARM)
hermesinterface.moveRightArm(jointAngles);
hermes_move_arm_action::MoveArmResult res;
res.return_value.val = arm_navigation_msgs::ArmNavigationErrorCodes::SUCCESS; // put in there some error code on errors
// this sends the response back to the caller
move_arm_action_server_.setSucceeded(res);
// if failed, use: move_arm_action_server_.setAborted(res);
}
/*!
* \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_)
{
PowerCubeCtrl::PC_CTRL_STATUS status;
std::vector<std::string> errorMessages;
ROS_WARN("here");
pc_ctrl_->getStatus(status, errorMessages);
ROS_WARN("here2");
std::cout << status << std::endl;
// @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))
{
ROS_ERROR("Skipping command: %s",pc_ctrl_->getErrorMessage().c_str());
return;
}
ROS_DEBUG("Executed velocity command");
}
else
{
ROS_ERROR("Skipping command: powercubes not initialized");
}
}
void StorePuckServer::controlLoop()
{
double vel_x = 0, vel_y = 0, vel_phi = 0;
ROS_DEBUG("%s", StorePuckStates::toString(state_).c_str());
double percentage = 0;
if (mode_ == store_modes::LASER_SCAN)
{
robotino_motion::AlignGoal frontal_alignment, lateral_alignment;
frontal_alignment.alignment_mode = 8; // FRONT_LASER alignment mode
frontal_alignment.distance_mode = 1; // NORMAL distance mode
//lateral_alignment.alignment_mode = 9; // RIGHT_LASER alignment mode
//lateral_alignment.distance_mode = 1; // NORMAL distance mode
align_client_.sendGoal(frontal_alignment);
state_ = store_puck_states::ALIGNING_FRONTAL;
align_client_.waitForResult();
ROS_INFO("Store_number_: %s", StoreStoreNumbers::toString(store_number_).c_str());
switch(store_number_)
{
case store_store_numbers::NEAR:
{
while(laser_front_ > 0.13)
{
setVelocity(0.15, 0, 0);
publishVelocity();
}
//setVelocity(0.0, 0.0, 0.0);
//publishVelocity();
}
break;
case store_store_numbers::MIDDLE:
{
printf("entrou no MIDDLE");
while(laser_front_ > 0.78)
{
setVelocity(0.15, 0, 0);
publishVelocity();
}
//setVelocity(0.0, 0.0, 0.0);
//publishVelocity();
}
break;
case store_store_numbers::FAR_AWAY:
{
printf("entrou no FAR_AWAY");
while(laser_front_ > 1.43)
{
setVelocity(0.15, 0, 0);
publishVelocity();
}
//setVelocity(0.0, 0.0, 0.0);
//publishVelocity();
}
break;
case store_store_numbers::VOLTAR_PRA_CASA:
{
frontal_alignment.alignment_mode = 8; // FRONT_LASER alignment mode
frontal_alignment.distance_mode = 1; // NORMAL distance mode
align_client_.sendGoal(frontal_alignment);
state_ = store_puck_states::ALIGNING_FRONTAL;
align_client_.waitForResult();
while(laser_front_ > 0.13)
{
setVelocity(0.15, 0, 0);
publishVelocity();
}
while(laser_left_ > 0.3)
{
setVelocity(0, 0.1, 0);
publishVelocity();
}
flag_aux_ = true;
}
break;
default:
ROS_ERROR("Store Number not supported yet!!!");
}
//laserDistanceFront(store_number_);
if(flag_aux_ == false)
{
frontal_alignment.alignment_mode = 8; // FRONT_LASER alignment mode
frontal_alignment.distance_mode = 1; // NORMAL distance mode
align_client_.sendGoal(frontal_alignment);
state_ = store_puck_states::ALIGNING_FRONTAL;
align_client_.waitForResult();
resetOdometry();
while(getOdometry_PHI() < PI / 2)
{
setVelocity(0, 0, 0.5);
publishVelocity();
}
frontal_alignment.alignment_mode = 8; // FRONT_LASER alignment mode
frontal_alignment.distance_mode = 1; // NORMAL distance mode
align_client_.sendGoal(frontal_alignment);
state_ = store_puck_states::ALIGNING_FRONTAL;
align_client_.waitForResult();
state_ = store_puck_states::STORING_PUCK;
while(laser_front_ > 0.33)
{
setVelocity(0.15, 0, 0);
publishVelocity();
}
state_ = store_puck_states::LEAVING_PUCK;
while(laser_front_ < 0.8)
{
setVelocity(-0.2, 0, 0);
publishVelocity();
}
if(laser_right_ < 0.6 || laser_left_ < 0.6)
{
while(laser_right_ < 0.6)
{
setVelocity(0, 0.2, 0);
publishVelocity();
}
while(laser_left_ < 0.6)
{
setVelocity(0, -0.2, 0);
publishVelocity();
}
}
}
}
else if (mode_ == store_modes::VISION)
{
if (state_ != store_puck_states::LEAVING_PUCK && state_ != store_puck_states::GOING_BACK_TO_ORIGIN)
{
robotino_vision::FindInsulatingTapeAreas srv;
find_areas_cli_.waitForExistence();
if (!find_areas_cli_.call(srv))
{
ROS_ERROR("Area not found!!!");
state_ = store_puck_states::LOST;
return;
}
std::vector<float> distances = srv.response.distances;
std::vector<float> directions = srv.response.directions;
int num_areas = srv.response.distances.size();
if (num_areas > 0 && state_ != store_puck_states::STORING_PUCK)
{
int closest_area_index = 0;
for (int i = 0; i < num_areas; i++)
{
if (distances[i] < distances[closest_area_index] && fabs(directions[i]) <fabs(directions[closest_area_index]))
{
closest_area_index = i;
}
}
double max_error_lateral = 50, max_error_frontal = 40;
double error_lateral = directions[closest_area_index];
if (error_lateral > max_error_lateral)
{
error_lateral = max_error_lateral;
}
double error_frontal = distances[closest_area_index];
if (error_frontal > max_error_frontal)
{
error_frontal = max_error_frontal;
}
float tolerance_lateral = 0.1, tolerance_frontal = 30;
double K_error_lateral = 0.3, K_error_frontal = 0.003;
double percentage_f, percentage_0, tolerance, max_error, error;
if (fabs(error_lateral) > tolerance_lateral) // 0% a 49%
{
state_ = store_puck_states::ALIGNING_LATERAL;
vel_y = -K_error_lateral * error_lateral;
percentage_0 = 0;
percentage_f = 49;
tolerance = tolerance_lateral;
max_error = max_error_lateral;
error = fabs(error_lateral);
}
else if (fabs(error_frontal) > tolerance_frontal) // 50% a 69%
{
state_ = store_puck_states::HEADING_TOWARD_AREA;
vel_x = K_error_frontal * error_frontal;
percentage_0 = 50;
percentage_f = 69;
tolerance = tolerance_frontal;
max_error = max_error_frontal;
error = fabs(error_frontal);
}
else
{
vel_x = .14;
state_ = store_puck_states::STORING_PUCK;
storing_start_ = ros::Time::now();
}
percentage = percentage_0 + (percentage_f - percentage_0) * (max_error - error) / (max_error - tolerance);
}
else if (state_ == store_puck_states::STORING_PUCK) // 70% a 89%
{
vel_x = .1;
double percentage_0 = 70, percentage_f = 89;
double elapsed_time = (ros::Time::now() - storing_start_).toSec();
if (elapsed_time > STORING_DEADLINE)
{
state_ = store_puck_states::LEAVING_PUCK;
}
percentage = percentage_0 + (percentage_f - percentage_0) * elapsed_time / STORING_DEADLINE;
}
}
else
{
if (state_ == store_puck_states::LEAVING_PUCK)
{
delta_x_ = -getOdometry_X();
vel_x = -.14;
resetOdometry();
state_ = store_puck_states::GOING_BACK_TO_ORIGIN;
percentage_ = 89;
}
double max_error_linear = 1.25;
double error_linear = delta_x_ - getOdometry_X();
if (error_linear > max_error_linear)
{
error_linear = max_error_linear;
}
float tolerance_linear = 0.01;
double K_error_linear = 1.2;
double percentage_f, percentage_0, tolerance, max_error, error;
if (fabs(error_linear) > tolerance_linear) // 90% a 99%
{
state_ = store_puck_states::GOING_BACK_TO_ORIGIN;
vel_x = K_error_linear * error_linear;
percentage_0 = 91;
percentage_f = 99;
tolerance = tolerance_linear;
max_error = max_error_linear;
error = fabs(error_linear);
}
percentage = percentage_0 + (percentage_f - percentage_0) * (max_error - error) / (max_error - tolerance);
}
}
else
{
ROS_ERROR("Storage Mode not supported yet!!!");
return;
}
if (vel_x == 0 && vel_y == 0 && vel_phi == 0) // 100%
{
state_ = store_puck_states::FINISHED;
percentage_ = 100;
}
if (percentage > percentage_)
{
percentage_ = percentage;
}
setVelocity(vel_x, vel_y, vel_phi);
publishVelocity();
publishFeedback();
}
void AprilMessageReceived(const apriltags::AprilTagDetections& detectionsMsg) {
double tmp_roll, tmp_pitch, tmp_yaw;
//AR frame: AprilTags Frame
//Q-frame: Quadrotor Frame
//if an AR tag is detected then get the positions, if not, then don't, so you don't segfault
if (&detectionsMsg.detections[0] != NULL){
CurrentPoseStamped.header = detectionsMsg.header;
//pose vector of quadrotor relative to AR tags as detected by the April Tags package
//These position coordinates have an error that needs to be corrected for, because the
//quadrotor yaw coordinates have an error relative to the April Tag fixed coordinate system
//so need to correct for that error
CurrentPoseStamped.pose.position.x = -detectionsMsg.detections[0].pose.position.x;
CurrentPoseStamped.pose.position.y = detectionsMsg.detections[0].pose.position.y;
CurrentPoseStamped.pose.position.z = detectionsMsg.detections[0].pose.position.z;
CurrentPoseStamped.pose.orientation.x = detectionsMsg.detections[0].pose.orientation.x;
CurrentPoseStamped.pose.orientation.y = detectionsMsg.detections[0].pose.orientation.y;
CurrentPoseStamped.pose.orientation.z = detectionsMsg.detections[0].pose.orientation.z;
CurrentPoseStamped.pose.orientation.w = detectionsMsg.detections[0].pose.orientation.w;
tf::Quaternion quat(CurrentPoseStamped.pose.orientation.x, CurrentPoseStamped.pose.orientation.y, CurrentPoseStamped.pose.orientation.z, CurrentPoseStamped.pose.orientation.w);
tf::Matrix3x3 m(quat);
m.getRPY(tmp_roll, tmp_pitch, tmp_yaw); //values from quadrotor in quadrotor original coordinates (need to be corrected for)
//**Converting ARTag coordinate position to Q-frame using yaw error correction
curr_Q_X = cos(curr_Q_Yaw)*CurrentPoseStamped.pose.position.x + sin(curr_Q_Yaw)*CurrentPoseStamped.pose.position.y + offset_X;
curr_Q_Y = -sin(curr_Q_Yaw)*CurrentPoseStamped.pose.position.x + cos(curr_Q_Yaw)*CurrentPoseStamped.pose.position.y + offset_Y;
curr_Q_Z = CurrentPoseStamped.pose.position.z + offset_Z;
//**Calculate yaw between AR frame and Q-frame from quaternions in detectionsMsg
//getting scalar of Yaw and converting from quarternion into ENU coordinate frame
curr_Q_Yaw = -tmp_yaw;
ROS_INFO("AprilTags Detected");
}else {
ROS_ERROR("No AprilTags Detected");
}
//**Calculate velocity from current and previous positions and delta_time_between_calls
// Time since last call
double delta_time_between_calls = (ros::Time::now() - prev_time).toSec();
prev_time = ros::Time::now();
// Calculate velocity
if (delta_time_between_calls < 1.0){
prev_vel_X = (prev_Q_X - curr_Q_X)/delta_time_between_calls;
prev_vel_Y = (prev_Q_Y - curr_Q_Y)/delta_time_between_calls;
prev_vel_Z = (prev_Q_Z - curr_Q_Z)/delta_time_between_calls;
} else {
prev_vel_X = 0.0;
prev_vel_Y = 0.0;
prev_vel_Z = 0.0;
}
//print info
/*char tab2[1024];
strncpy(tab2, mode.c_str(), sizeof(tab2));
tab2[sizeof(tab2) - 1] = 0;
ROS_INFO("Quadrotor_Curr_Coordinates = (%f , %f) | Previous_Coordinates = (%f , %f) \n delta_time_between_calls = %fs | prev_vel_X = (%f , %f)\n Roll = %f | Pitch = %f\n Mode = %s \n", curr_Q_X, curr_Q_Y, prev_Q_X, prev_Q_Y, delta_time_between_calls, prev_vel_X, prev_vel_Y, Roll, Pitch, tab2);
*/
// Error between Quadrotor coordinates and April Tag coordinates in ENU (East North Up = X Y Z)
float ErX = 0.0;
float ErY = 0.0;
float ErZ = 0.0;
float ErYaw = 0.0;
prev_Q_X = curr_Q_X;
prev_Q_Y = curr_Q_Y;
prev_Q_Z = curr_Q_Z;
// Calculate the error between Quadrotor center and ARTag center
ErX = des_X - curr_Q_X;
ErY = des_Y - curr_Q_Y;
ErZ = des_Z - curr_Q_Z;
ErYaw = des_Yaw - curr_Q_Yaw;
// Calculate Roll, Pitch, Throttle, Yaw depending on the mode
if (mode == "ALT_HOLD"){
Roll = BASERC + FACTOR_ROLL*(0.5*ErX+0.1*prev_vel_X);
Pitch = BASERC + FACTOR_PITCH*(0.5*ErY+0.1*prev_vel_Y);
Throttle = BASERC + FACTOR_THROTTLE*(0.5*ErZ+0.1*prev_vel_Z);
Yaw = BASERC + FACTOR_YAW*(0.1*ErYaw);
} else{
Roll = BASERC;
Pitch = BASERC;
Throttle = BASERC;
Yaw = BASERC;
}
// Limit the Roll
if (Roll > MAXRC){
Roll = MAXRC;
} else if (Roll < MINRC){
Roll = MINRC;
}
// Limit the Pitch
if (Pitch > MAXRC){
Pitch = MAXRC;
} else if (Pitch < MINRC){
Pitch = MINRC;
}
// Limit the Throttle
if (Throttle > MAXRC){
Throttle = MAXRC;
} else if (Throttle < MINRC){
Throttle = MINRC;
}
// Limit the Yaw
if (Yaw > MAXRC){
Yaw = MAXRC;
} else if (Yaw < MINRC){
Yaw = MINRC;
}
//Publish new values to msg.channels[] as shown below for quadrotor flight controller
msg.channels[0] = Roll; //Roll
msg.channels[1] = Pitch; //Pitch
msg.channels[2] = Throttle; //Throttle
msg.channels[3] = Yaw; //Yaw
msg.channels[4] = 0;
msg.channels[5] = 0;
msg.channels[6] = 0;
msg.channels[7] = 0;
ROS_INFO("ROLL: %0.1f PITCH: %0.1f THROTTLE: %0.1f YAW: %0.1f", Roll, Pitch, Throttle, Yaw);
pub.publish(msg);
}
