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); }