String TreeDlg::getDerivation(CTreeCtrl *ctrl, HTREEITEM item, int maxlen) { String result; getDerivation(ctrl, item, result, maxlen+10); if((int)result.length() > maxlen) { result = left(result, maxlen-3) + _T("..."); } return result; }
BOOL TreeDlg::PreTranslateMessage(MSG *pMsg) { if(TranslateAccelerator(m_hWnd, m_accelTable, pMsg)) { return true; } switch(pMsg->message) { case WM_RBUTTONDOWN: { CTreeCtrl *ctrl = getTreeCtrl(); CPoint p = pMsg->pt; ctrl->ScreenToClient(&p); HTREEITEM item = findTreeItemByPoint(ctrl, p); if(item != NULL) { const String derivation = getDerivation(ctrl, item, 160); showInformation(derivation); return TRUE; } } break; } return __super::PreTranslateMessage(pMsg); }
void TreeDlg::getDerivation(CTreeCtrl *ctrl, HTREEITEM item, String &derivation, int maxlen) { int image, selectedimage; ctrl->GetItemImage(item, image, selectedimage); if(image == 0) { // item is a terminal CString s = ctrl->GetItemText(item); maxlen -= (int)derivation.length(); if(maxlen < 0) return; derivation += s.GetBuffer(maxlen); return; } for(HTREEITEM child = ctrl->GetChildItem(item); child != NULL; child = ctrl->GetNextSiblingItem(child)) { // item is a nonterminal String tmp; getDerivation(ctrl, child, tmp, maxlen); if(derivation.length() > 0) derivation += _T(" "); derivation += tmp; if((int)derivation.length() > maxlen) { derivation = left(derivation, maxlen); return; } } }
void callback(const gazebo_msgs::ModelStates::ConstPtr& msg) { // ModelStates does not have a timestamp: work around: use own time!! //throttle to about 20 Hz as Gazebo publishes at 1000Hz ros::Time time_now = ros::Time::now(); static ros::Time time_last = time_now; ros::Duration dur_last = time_now - time_last; if (dur_last >= sleep_dur) { tf::Transform obj_t; static tf::Transform obj_t_old; tf::Transform rob; float vel_now; static float vel_old; float acc_now; static float acc_old; float jerk_now; visualization_msgs::MarkerArray marker_array; std::vector<tf::Transform> poses = getObjectPose(msg, object_name); rob = poses.at(0); obj_t = poses.at(1); //ROS_INFO("object origin: %4.2f,%4.2f",obj_t.getOrigin().getX(), obj_t.getOrigin().getY()); //Object frame and mesh tf::StampedTransform tr( //gaz2map_ * obj_t, obj_t, //rob.inverse() * obj_t * rob2try.inverse(), time_now, //std::string("/map"), std::string("/gazebo_link"), //std::string("/base_tray_link"), std::string("/tray_object_1")); br.sendTransform(tr); marker_array.markers.push_back(makeMarker(tr, std::string("object_model"))); // Reference angle tf::Quaternion ref_ang(0,0,0,1); //ROS_INFO("Z axis: %4.2f, %4.2f, %4.2f",/*{{{*/ // tf::Matrix3x3(ref_ang).getColumn(2).getX(), // tf::Matrix3x3(ref_ang).getColumn(2).getY(), // tf::Matrix3x3(ref_ang).getColumn(2).getZ());/*}}}*/ ref_ang.setRPY(0,-M_PI/2,0); tf::Transform obj_ref = gaz2map_ * tf::Transform(ref_ang, obj_t.getOrigin()); marker_array.markers.push_back(makeMarker(tf::StampedTransform( obj_ref, time_now, std::string("/map"), std::string("/tray_object")), std::string("ref_angle"), true)); // actual object angle tf::Transform obj_ang = gaz2map_ * obj_t;// * tf::Transform(ref_ang, tf::Vector3(0,0,0)); tf::Vector3 z_ax_obj( tf::Matrix3x3(obj_ang.getRotation()).getColumn(2).getX(), tf::Matrix3x3(obj_ang.getRotation()).getColumn(2).getY(), tf::Matrix3x3(obj_ang.getRotation()).getColumn(2).getZ()); //ROS_INFO("Z axis obj: %4.2f, %4.2f, %4.2f",/*{{{*/ // z_ax_obj.getX(), // z_ax_obj.getY(), // z_ax_obj.getZ());/*}}}*/ marker_array.markers.push_back(makeMarker(tf::StampedTransform( obj_ang * tf::Transform(ref_ang, tf::Vector3(0,0,0)), time_now, std::string("/map"), std::string("/tray_object")), std::string("obj_angle"), true)); // publish the marker mark_pub->publish(marker_array); //Angle between reference vector and object vector (z) double ang = acos( z_ax_obj.getZ() / sqrt( +z_ax_obj.getX()*z_ax_obj.getX() +z_ax_obj.getY()*z_ax_obj.getY() +z_ax_obj.getZ()*z_ax_obj.getZ())); //ROS_INFO("Angle: %4.2f", ang); geometry_msgs::Vector3Stamped dev_msg; dev_msg.header.stamp = time_now; dev_msg.header.frame_id = "/base_footprint"; dev_msg.vector.x = ang; //relative translatory deviation object to tray tf::Vector3 tray_obj = obj_t.getOrigin() - (rob*rob2obj).getOrigin(); //vector length is trans deviation dev_msg.vector.y = sqrt( +tray_obj.getX()*tray_obj.getX() +tray_obj.getY()*tray_obj.getY() +tray_obj.getZ()*tray_obj.getZ()); dev_pub->publish(dev_msg); // object dynamcis vel_now = getVel(obj_t.getOrigin(), obj_t_old.getOrigin(), dur_last); vel_now = getMovingAverage(vel_now); acc_now = getDerivation(vel_now, vel_old, dur_last); jerk_now = getDerivation(acc_now, acc_old, dur_last); float orient = getOrient(obj_t, obj_t_old); //ROS_INFO("Obj. dynamics: %4.2f,%4.2f,%4.2f,%4.2f", vel_now, acc_now, jerk_now, orient); std::vector<float> dyn = {vel_now, acc_now, jerk_now, orient}; publishDynamics(dyn, time_now); obj_t_old = obj_t; vel_old = vel_now; acc_old = acc_now; time_last = time_now; } }