【id:34】【20分】D. Point_Array(类+构造+对象数组)

博客要求在原C++代码基础上进行修改,包括增加自写析构函数、修改getDisTo方法参数、修改构造函数。还需在主函数根据用户输入建立Point数组,求出数组内距离最大的两个点的距离值,并给出了C++输出指定精度的参考代码。

题目描述

上面是我们曾经练习过的一个习题,请在原来代码的基础上作以下修改:1、增加自写的析构函数;2、将getDisTo方法的参数修改为getDisTo(const Point &p);3、根据下面输出的内容修改相应的构造函数。

然后在主函数中根据用户输入的数目建立Point数组,求出数组内距离最大的两个点之间的距离值。

输入

测试数据的组数 t

第一组点的个数

第一个点的 x 坐标 y坐标

第二个点的 x坐标 y坐标

............

输出

输出第一组距离最大的两个点以及其距离(存在多个距离都是最大值的情况下,输出下标排序最前的点组合。比如如果p[0]和p[9]、p[4]和p[5]之间的距离都是最大值,那么前一个是答案,因为p[0]排序最前)

...........

在C++中,输出指定精度的参考代码如下:

#include <iostream>

#include <iomanip> //必须包含这个头文件

using namespace std;

void main( )

{ double a =3.141596;

cout<<fixed<<setprecision(3)<<a<<endl; //输出小数点后3位

样例查看模式

正常显示查看格式

输入样例1 <-复制

输出样例1

语言: 编译选项

主题:

#include<iostream>
#include <iomanip> //必须包含这个头文件
using namespace std;
class point
{
    double x, y;
public:
    point();
    point(double x_value, double y_value);
    ~point();
    double getX();
    double getY();
    void setXY(double x1, double y1) { x = x1; y = y1; }
    void setX(double x_value);
    void setY(double y_value);
    double getDisTo(point& p);
};
point::point()
{
    x = 0;
    y = 0;
    cout << "Constructor." << endl;
}
point::point(double x_value, double y_value)
{
    x = x_value;
    y = y_value;
    cout << "Constructor." << endl;
}
point::~point()
{
    cout << "Distructor." << endl;//析构函数
}
double point::getX()
{
    return x;
}
double point::getY()
{
    return y;
}
void point::setX(double x_value)
{
    x = x_value;
}
void point::setY(double y_value)
{
    y = y_value;
}
double point::getDisTo(point& p)
{
    return sqrt((x - p.x) * (x - p.x) + (y - p.y) * (y - p.y));
}
int main()
{
    int t;
    cin >> t;
    int index1 = 0;
    int index2 = 0;
    //数目建立Point数组,求出数组内距离最大的两个点之间的距离值。
    while (t--)
    {
        int times = 0;
        cin >> times;
        point* p = new point[times];//创建动态数组
        double x, y;
        for (int i = 0; i < times; i++)
        {
            cin >> x;
            cin >> y;
            p[i].setXY(x, y);//数据录入
        }
        double max = 0;
        for (int i = 0; i < times; i++)
        {
            for (int j = i + 1; j < times; j++)
            {
                if (p[i].getDisTo(p[j])>max)//距离最大的
                {
                    max = p[i].getDisTo(p[j]);
                    index1 = i;
                    index2 = j;
                }
            }
        }
        cout << "The longeset distance is " << fixed << setprecision(2) << max << ",between p[" << index1 << "] and p[" << index2 << "]." << endl;
        delete[] p;
    }
    return 0;
}
public: Visualization(ros::NodeHandle &nh) : nh_(nh) {} template <class CENTER, class TOPIC> void visualize_a_ball(const CENTER &c, const double &r, const TOPIC &topic, const Color color = blue, const double a = 1) { auto got = publisher_map_.find(topic); if (got == publisher_map_.end()) { ros::Publisher pub = nh_.advertise<visualization_msgs::Marker>(topic, 10); publisher_map_[topic] = pub; } visualization_msgs::Marker marker; marker.header.frame_id = "map"; marker.type = visualization_msgs::Marker::SPHERE; marker.action = visualization_msgs::Marker::ADD; setMarkerColor(marker, color, a); setMarkerScale(marker, 2 * r, 2 * r, 2 * r); setMarkerPose(marker, c[0], c[1], c[2]); marker.header.stamp = ros::Time::now(); publisher_map_[topic].publish(marker); } template <class PC, class TOPIC> void visualize_pointcloud(const PC &pc, const TOPIC &topic) { auto got = publisher_map_.find(topic); if (got == publisher_map_.end()) { ros::Publisher pub = nh_.advertise<sensor_msgs::PointCloud2>(topic, 10); publisher_map_[topic] = pub; } pcl::PointCloud<pcl::PointXYZ> point_cloud; sensor_msgs::PointCloud2 point_cloud_msg; point_cloud.reserve(pc.size()); for (const auto &pt : pc) { point_cloud.points.emplace_back(pt[0], pt[1], pt[2]); } pcl::toROSMsg(point_cloud, point_cloud_msg); point_cloud_msg.header.frame_id = "map"; point_cloud_msg.header.stamp = ros::Time::now(); publisher_map_[topic].publish(point_cloud_msg); } template <class PATH, class TOPIC> void visualize_path(const PATH &path, const TOPIC &topic) { auto got = publisher_map_.find(topic); if (got == publisher_map_.end()) { ros::Publisher pub = nh_.advertise<nav_msgs::Path>(topic, 10); publisher_map_[topic] = pub; } nav_msgs::Path path_msg; geometry_msgs::PoseStamped tmpPose; tmpPose.header.frame_id = "map"; for (const auto &pt : path) { tmpPose.pose.position.x = pt[0]; tmpPose.pose.position.y = pt[1]; tmpPose.pose.position.z = pt[2]; path_msg.poses.push_back(tmpPose); } path_msg.header.frame_id = "map"; path_msg.header.stamp = ros::Time::now(); publisher_map_[topic].publish(path_msg); } template <class BALLS, class TOPIC> void visualize_balls(const BALLS &balls, const TOPIC &topic, const Color color = blue, const double a = 0.2) { auto got = publisher_map_.find(topic); if (got == publisher_map_.end()) { ros::Publisher pub = nh_.advertise<visualization_msgs::MarkerArray>(topic, 10); publisher_map_[topic] = pub; } visualization_msgs::Marker marker; marker.header.frame_id = "map"; marker.type = visualization_msgs::Marker::SPHERE; marker.action = visualization_msgs::Marker::ADD; marker.id = 0; setMarkerColor(marker, color, a); visualization_msgs::MarkerArray marker_array; marker_array.markers.reserve(balls.size() + 1); marker.action = visualization_msgs::Marker::DELETEALL; marker_array.markers.push_back(marker); marker.action = visualization_msgs::Marker::ADD; for (const auto &ball : balls) { setMarkerPose(marker, ball.center[0], ball.center[1], ball.center[2]); auto d = 2 * ball.radius; setMarkerScale(marker, d, d, d); marker_array.markers.push_back(marker); marker.id++; } publisher_map_[topic].publish(marker_array); } template <class ELLIPSOIDS, class TOPIC> void visualize_ellipsoids(const ELLIPSOIDS &ellipsoids, const TOPIC &topic, const Color color = blue, const double a = 0.2) { auto got = publisher_map_.find(topic); if (got == publisher_map_.end()) { ros::Publisher pub = nh_.advertise<visualization_msgs::MarkerArray>(topic, 10); publisher_map_[topic] = pub; } visualization_msgs::Marker marker; marker.header.frame_id = "map"; marker.type = visualization_msgs::Marker::SPHERE; marker.action = visualization_msgs::Marker::ADD; marker.id = 0; setMarkerColor(marker, color, a); visualization_msgs::MarkerArray marker_array; marker_array.markers.reserve(ellipsoids.size() + 1); marker.action = visualization_msgs::Marker::DELETEALL; marker_array.markers.push_back(marker); marker.action = visualization_msgs::Marker::ADD; for (const auto &e : ellipsoids) { setMarkerPose(marker, e.c[0], e.c[1], e.c[2], e.R); setMarkerScale(marker, 2 * e.rx, 2 * e.ry, 2 * e.rz); marker_array.markers.push_back(marker); marker.id++; } publisher_map_[topic].publish(marker_array); } template <class PAIRLINE, class TOPIC> // eg for PAIRLINE: std::vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>> void visualize_pairline(const PAIRLINE &pairline, const TOPIC &topic, const Color &color = green, double scale = 0.1) { auto got = publisher_map_.find(topic); if (got == publisher_map_.end()) { ros::Publisher pub = nh_.advertise<visualization_msgs::Marker>(topic, 10); publisher_map_[topic] = pub; } visualization_msgs::Marker marker; marker.header.frame_id = "map"; marker.type = visualization_msgs::Marker::LINE_LIST; marker.action = visualization_msgs::Marker::ADD; setMarkerPose(marker, 0, 0, 0); setMarkerColor(marker, color, 1); setMarkerScale(marker, scale, scale, scale); marker.points.resize(2 * pairline.size()); for (size_t i = 0; i < pairline.size(); ++i) { marker.points[2 * i + 0].x = pairline[i].first[0]; marker.points[2 * i + 0].y = pairline[i].first[1]; marker.points[2 * i + 0].z = pairline[i].first[2]; marker.points[2 * i + 1].x = pairline[i].second[0]; marker.points[2 * i + 1].y = pairline[i].second[1]; marker.points[2 * i + 1].z = pairline[i].second[2]; } publisher_map_[topic].publish(marker); } template <class ARROWS, class TOPIC> // ARROWS: pair<Vector3d, Vector3d> void visualize_arrows(const ARROWS &arrows, const TOPIC &topic, const Color &color) { auto got = publisher_map_.find(topic); if (got == publisher_map_.end()) { ros::Publisher pub = nh_.advertise<visualization_msgs::MarkerArray>(topic, 10); publisher_map_[topic] = pub; } visualization_msgs::Marker clear_previous_msg; clear_previous_msg.action = visualization_msgs::Marker::DELETEALL; visualization_msgs::Marker arrow_msg; arrow_msg.type = visualization_msgs::Marker::ARROW; arrow_msg.action = visualization_msgs::Marker::ADD; arrow_msg.header.frame_id = "map"; arrow_msg.id = 0; arrow_msg.points.resize(2); setMarkerPose(arrow_msg, 0, 0, 0); setMarkerScale(arrow_msg, 0.4, 0.7, 0); setMarkerColor(arrow_msg, color, 0.7); visualization_msgs::MarkerArray arrow_list_msg; arrow_list_msg.markers.reserve(1 + arrows.size()); arrow_list_msg.markers.push_back(clear_previous_msg); for (const auto &arrow : arrows) { arrow_msg.points[0].x = arrow.first[0]; arrow_msg.points[0].y = arrow.first[1]; arrow_msg.points[0].z = arrow.first[2]; arrow_msg.points[1].x = arrow.second[0]; arrow_msg.points[1].y = arrow.second[1]; arrow_msg.points[1].z = arrow.second[2]; arrow_list_msg.markers.push_back(arrow_msg); arrow_msg.id += 1; } publisher_map_[topic].publish(arrow_list_msg); } template <class TRAJ, class TOPIC> // TRAJ: void visualize_traj(const TRAJ &traj, const TOPIC &topic) { std::vector<Eigen::Vector3d> path; auto duration = traj.getTotalDuration(); for (double t = 0; t < duration; t += 0.01) { path.push_back(traj.getPos(t)); } visualize_path(path, topic); std::vector<Eigen::Vector3d> wayPts; for (const auto &piece : traj) { wayPts.push_back(piece.getPos(0)); } visualize_pointcloud(wayPts, std::string(topic) + "_wayPts"); } template <class TRAJLIST, class TOPIC> // TRAJLIST: std::vector<TRAJ> void visualize_traj_list(const TRAJLIST &traj_list, const TOPIC &topic, const Color color = blue, double scale = 0.1) { auto got = publisher_map_.find(topic); if (got == publisher_map_.end()) { ros::Publisher pub = nh_.advertise<visualization_msgs::MarkerArray>(topic, 10); publisher_map_[topic] = pub; } visualization_msgs::Marker clear_previous_msg; clear_previous_msg.action = visualization_msgs::Marker::DELETEALL; visualization_msgs::Marker path_msg; path_msg.type = visualization_msgs::Marker::LINE_STRIP; path_msg.action = visualization_msgs::Marker::ADD; path_msg.header.frame_id = "map"; path_msg.id = 0; setMarkerPose(path_msg, 0, 0, 0); setMarkerScale(path_msg, scale, scale, scale); visualization_msgs::MarkerArray path_list_msg; path_list_msg.markers.reserve(1 + traj_list.size()); path_list_msg.markers.push_back(clear_previous_msg); double a_step = 0.8 / traj_list.size(); double a = 1.0; geometry_msgs::Point p_msg; for (const auto &traj : traj_list) { setMarkerColor(path_msg, color, a); // a = a + a_step; path_msg.points.clear(); for (double t = 0; t < traj.getTotalDuration(); t += 0.01) { auto p = traj.getPos(t); p_msg.x = p.x(); p_msg.y = p.y(); p_msg.z = p.z(); path_msg.points.push_back(p_msg); } path_list_msg.markers.push_back(path_msg); path_msg.id += 1; } publisher_map_[topic].publish(path_list_msg); } template <class PATHLIST, class TOPIC> // PATHLIST: std::vector<PATH> void visualize_path_list(const PATHLIST &path_list, const TOPIC &topic, const Color color = steelblue, double scale = 0.1) { auto got = publisher_map_.find(topic); if (got == publisher_map_.end()) { ros::Publisher pub = nh_.advertise<visualization_msgs::MarkerArray>(topic, 10); publisher_map_[topic] = pub; } visualization_msgs::Marker clear_previous_msg; clear_previous_msg.action = visualization_msgs::Marker::DELETEALL; visualization_msgs::Marker path_msg; path_msg.type = visualization_msgs::Marker::LINE_STRIP; path_msg.action = visualization_msgs::Marker::ADD; path_msg.header.frame_id = "map"; path_msg.id = 0; setMarkerPose(path_msg, 0, 0, 0); setMarkerScale(path_msg, scale, scale, scale); visualization_msgs::MarkerArray path_list_msg; path_list_msg.markers.reserve(1 + path_list.size()); path_list_msg.markers.push_back(clear_previous_msg); setMarkerColor(path_msg, color); for (const auto &path : path_list) { path_msg.points.resize(path.size()); for (size_t i = 0; i < path.size(); ++i) { path_msg.points[i].x = path[i].x(); path_msg.points[i].y = path[i].y(); path_msg.points[i].z = path[i].z(); } path_list_msg.markers.push_back(path_msg); path_msg.id += 1; } publisher_map_[topic].publish(path_list_msg); } template <class TOPIC_TYPE, class TOPIC> void registe(const TOPIC& topic) { auto got = publisher_map_.find(topic); if (got == publisher_map_.end()) { ros::Publisher pub = nh_.advertise<TOPIC_TYPE>(topic, 10); publisher_map_[topic] = pub; } } }; 注释上述代码
09-19
帮我改下面的函数,功能相同,但不要使用递归的方法bool OtsTrack::BackTrackForArray(int startSearchingID, int *inner_id, std::vector<int> &array_temp, std::vector<int> &inner_temp, double **distancesOfAllPoints, VecPoint3D &pts3d_, VecPoint3D &arrayPoints, int &inner_pointer, std::vector<int> &inner_id_in_array, std::vector<int> &indexes) { std::string realArrayID = ""; int pointsSize = -1; int virtualID = array_temp[indexes[0]]; if (virtualID != -1) { int virtualArrayID = m_pointsMapArrayID[virtualID]; realArrayID = m_arrayMarkerIds[virtualArrayID]; pointsSize = m_mapToolSetting.at(realArrayID).FaceData[m_pointsMapFaceID[virtualID]].FaceMarkerNum; if (inner_pointer >= pointsSize) return true; } // find a point Point3D p1; int index = 0; // the the distance between first point and current point double distance = 0.0; for (size_t i = startSearchingID; i < pts3d_.size(); i++) { bool hasSameIndex = false; for (auto in : indexes) if (in == i) hasSameIndex = true; if (hasSameIndex || array_temp[i] != -1) continue; p1 = pts3d_[i]; index = i; if (fabs(distancesOfAllPoints[indexes[0]][i]) < 1e-5) { distance = (arrayPoints[0] - p1).Norm2(); distancesOfAllPoints[indexes[0]][i] = distance; } else distance = distancesOfAllPoints[indexes[0]][i]; if (distance > m_fMaxRefDistance) { continue; } // set indexes.emplace_back(index); arrayPoints.emplace_back(p1); if (inner_pointer == 1) { std::vector<int> spareArray; std::vector<int> inner_wrongConfirmed_id_1; //<0,1,2,3 std::vector<int> inner_wrongConfirmed_id_2; for (int k = 0; k < m_pointsMapArrayID.size(); k++) { for (int j1 = 0; j1 < m_AllRefDistances[k].size(); j1++) for (int j2 = j1 + 1; j2 < m_AllRefDistances[k].size(); j2++) { double diff_12 = fabs(m_AllRefDistances[k][j1][j2] - distance); if (diff_12 < m_OTSParam.fMatchingDistanceThreshold) { // less than m_OTSParam.fMatchingDistanceThreshold mm spareArray.emplace_back(k); inner_wrongConfirmed_id_1.emplace_back(j1); inner_wrongConfirmed_id_2.emplace_back(j2); } } } for (size_t h = 0; h < spareArray.size(); h++) { //<first and second point // set inner_id[0] = inner_wrongConfirmed_id_1[h]; inner_id[1] = inner_wrongConfirmed_id_2[h]; array_temp[indexes[0]] = spareArray[h]; array_temp[indexes[inner_pointer++]] = spareArray[h]; if (BackTrackForArray(i + 1, inner_id, array_temp, inner_temp, distancesOfAllPoints, pts3d_, arrayPoints, inner_pointer, inner_id_in_array, indexes)) { return true; } // resume inner_id[0] = -1; inner_id[1] = -1; array_temp[indexes[0]] = -1; array_temp[indexes[--inner_pointer]] = -1; } // resume arrayPoints.pop_back(); indexes.pop_back(); } // confirm the inner id else if (inner_pointer == 2) { std::vector<int> inner_wrongConfirmed_id_1; //<0,1,2,3 std::vector<int> numberofInner; std::vector<int> inner_wrongConfirmed_id_2; for (int q = 0; q < 2; q++) { for (int j2 = 0; j2 < pointsSize; j2++) { bool haveSameInnerId = false; for (int ind = 0; ind < inner_pointer; ind++) { if (inner_id[ind] == j2) { haveSameInnerId = true; break; } } if (haveSameInnerId) // continue; double diff_13 = fabs(m_AllRefDistances[virtualID][inner_id[q]][j2] - distance); if (diff_13 < m_OTSParam.fMatchingDistanceThreshold) { numberofInner.emplace_back(q); inner_wrongConfirmed_id_1.emplace_back(inner_id[q]); inner_wrongConfirmed_id_2.emplace_back(j2); } } } if (numberofInner.empty()) { arrayPoints.pop_back(); indexes.pop_back(); continue; } // bool matched = true; Point3D p2 = arrayPoints[1]; //<second point double distance_p1p2 = 0.0; if (fabs(distancesOfAllPoints[indexes[1]][index]) < 1e-5) { distance_p1p2 = (p2 - p1).Norm2(); distancesOfAllPoints[indexes[1]][index] = distance_p1p2; } else distance_p1p2 = distancesOfAllPoints[indexes[1]][index]; for (size_t h = 0; h < numberofInner.size(); h++) { int index_array_2 = numberofInner[h] == 0 ? 1 : 0; double diff_23 = fabs(m_AllRefDistances[virtualID][inner_id[index_array_2]][inner_wrongConfirmed_id_2[h]] - distance_p1p2); if (diff_23 < m_OTSParam.fMatchingDistanceThreshold) { inner_id_in_array.emplace_back(inner_id[numberofInner[h]]); // A inner_id_in_array.emplace_back(inner_id[index_array_2]); // B inner_id_in_array.emplace_back(inner_wrongConfirmed_id_2[h]); inner_temp[index] = inner_wrongConfirmed_id_2[h]; inner_id[inner_pointer++] = inner_wrongConfirmed_id_2[h]; array_temp[index] = virtualID; inner_temp[indexes[0]] = inner_id_in_array[0]; // first point inner_temp[indexes[1]] = inner_id_in_array[1]; // second point } else continue; bool angleMatched = AngleMatching(arrayPoints, inner_id_in_array, pointsSize, virtualID); if (!angleMatched) { // resume inner_temp[indexes[1]] = -1; inner_temp[indexes[0]] = -1; inner_temp[index] = -1; inner_id[--inner_pointer] = -1; array_temp[index] = -1; inner_id_in_array.pop_back(); inner_id_in_array.pop_back(); inner_id_in_array.pop_back(); // arrayPoints.pop_back(); // indexes.pop_back(); continue; } if (BackTrackForArray(i + 1, inner_id, array_temp, inner_temp, distancesOfAllPoints, pts3d_, arrayPoints, inner_pointer, inner_id_in_array, indexes)) { return true; } // resume inner_temp[indexes[1]] = -1; inner_temp[indexes[0]] = -1; inner_temp[index] = -1; inner_id[--inner_pointer] = -1; array_temp[index] = -1; inner_id_in_array.pop_back(); inner_id_in_array.pop_back(); inner_id_in_array.pop_back(); } arrayPoints.pop_back(); indexes.pop_back(); } else if (inner_pointer >= 3) { // if(inner_temp[indexes[0]] == -1){ // return false; // } for (int j2 = 0; j2 < pointsSize; j2++) { bool haveSameInnerId = false; for (int ind = 0; ind < inner_pointer; ind++) { if (inner_id_in_array[ind] == j2) { haveSameInnerId = true; break; } } if (haveSameInnerId) // continue; double diff = fabs(m_AllRefDistances[virtualID][inner_temp[indexes[0]]][j2] - distance); if (diff < m_OTSParam.fMatchingDistanceThreshold) { //< third point // set inner_id[inner_pointer++] = j2; //<inner_pointer = 4 inner_temp[index] = j2; array_temp[index] = virtualID; inner_id_in_array.emplace_back(j2); // D break; } } if (inner_pointer < arrayPoints.size()) { // this is no any operation on inner_id, inner_temp,inner_id_in_array arrayPoints.pop_back(); indexes.pop_back(); continue; } // cross validation bool matched = true; for (int j = 1; j < inner_pointer - 1; j++) { Point3D p2 = arrayPoints[j]; double distance_p1p2 = 0.0; if (fabs(distancesOfAllPoints[indexes[j]][index]) < 1e-5) { distance_p1p2 = (p2 - p1).Norm2(); distancesOfAllPoints[indexes[j]][index] = distance_p1p2; } else distance_p1p2 = distancesOfAllPoints[indexes[j]][index]; double diff = fabs(m_AllRefDistances[virtualID][inner_temp[indexes[j]]][inner_temp[index]] - distance_p1p2); if (diff > m_OTSParam.fMatchingDistanceThreshold) { matched = false; break; } } // resume if (!matched) { //< recover the data inner_pointer--; // inner_pointer = 3 /* As for disambiguity and array matching, there is no need to match every edges and angles. */ inner_id[inner_pointer] = -1; inner_temp[index] = -1; array_temp[index] = -1; inner_id_in_array.pop_back(); // D arrayPoints.pop_back(); indexes.pop_back(); continue; } if (BackTrackForArray(i + 1, inner_id, array_temp, inner_temp, distancesOfAllPoints, pts3d_, arrayPoints, inner_pointer, inner_id_in_array, indexes)) { return true; } // resume inner_pointer--; inner_id[inner_pointer] = -1; inner_temp[index] = -1; array_temp[index] = -1; inner_id_in_array.pop_back(); arrayPoints.pop_back(); indexes.pop_back(); } } if (arrayPoints.size() >= 3 && !realArrayID.empty()) { if (inner_pointer >= m_mapToolSetting.at(realArrayID).MinFaceMarkerNum) return true; } return false; }
11-14
#include <ros/ros.h> #include <actionlib/server/simple_action_server.h> #include <control_msgs/FollowJointTrajectoryAction.h> #include <sensor_msgs/JointState.h> #include <thread> #include <mutex> #include <atomic> #include <array> #include <vector> #include <future> #include "motor5_pkg/motor.h" #include "motor5_pkg/deccanopenapi.h" #include "motor5_pkg/controlcan.h" class FollowJointTrajectoryAction { public: explicit FollowJointTrajectoryAction(const std::string& name); ~FollowJointTrajectoryAction(); void enableMotors(); void goalCB(); void preemptCB(); void publishJointStatesCb(const ros::WallTimerEvent&); void readerLoop(); void setupTPDOListeners(); std::array<double, 5> getCurrentPositions(); void executeTrajectory(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal); void trajectoryTimerCallback(const ros::TimerEvent&); private: ros::NodeHandle nh_; actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> as_; std::string action_name_; Motor motor1, motor2, motor3, motor4, motor5; ros::Publisher joint_pub_; ros::WallTimer joint_timer_; std::vector<std::string> joint_names_ = {"joint1", "joint2", "joint3", "joint4", "joint5"}; std::mutex sdk_mtx_; std::array<double, 5> pos_cache_{{0, 0, 0, 0, 0}}; std::mutex pos_mtx_; std::thread reader_thread_; std::atomic<bool> reader_running_{false}; ros::Timer trajectory_timer_; std::atomic<bool> trajectory_active_{false}; std::atomic<size_t> current_trajectory_point_{0}; control_msgs::FollowJointTrajectoryGoalConstPtr current_goal_; }; void FollowJointTrajectoryAction::enableMotors() { const int delayMicroseconds = 20000; // 0.02 秒 std::array<Motor*, 5> motors = {&motor1, &motor2, &motor3, &motor4, &motor5}; for (auto motor : motors) { motor->enable(true); usleep(delayMicroseconds); } } void FollowJointTrajectoryAction::setupTPDOListeners() { try { motor1.initTPDOListener(); motor2.initTPDOListener(); motor3.initTPDOListener(); motor4.initTPDOListener(); motor5.initTPDOListener(); } catch (const std::exception& e) { ROS_WARN("TPDO failed: %s", e.what()); } } std::array<double, 5> FollowJointTrajectoryAction::getCurrentPositions() { std::array<double, 5> vals{}; std::vector<std::future<void>> futures; for (size_t i = 0; i < 5; ++i) { futures.push_back(std::async(std::launch::async, [this, &vals, i] { try { std::lock_guard<std::mutex> lk(sdk_mtx_); switch (i) { case 0: vals[i] = motor1.getCurrentPos(); break; case 1: vals[i] = motor2.getCurrentPos(); break; case 2: vals[i] = motor3.getCurrentPos(); break; case 3: vals[i] = motor4.getCurrentPos(); break; case 4: vals[i] = motor5.getCurrentPos(); break; } } catch (const std::exception& e) { ROS_WARN_THROTTLE(1.0, "read positions error: %s", e.what()); } })); } for (auto& future : futures) { future.get(); } return vals; } void FollowJointTrajectoryAction::executeTrajectory(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal) { trajectory_active_ = true; current_trajectory_point_ = 0; current_goal_ = goal; trajectory_timer_ = nh_.createTimer(ros::Duration(0.02), &FollowJointTrajectoryAction::trajectoryTimerCallback, this); } void FollowJointTrajectoryAction::trajectoryTimerCallback(const ros::TimerEvent& e) { if (!trajectory_active_ || !current_goal_) return; ros::Time now = ros::Time::now(); const auto& traj = current_goal_->trajectory; if (current_trajectory_point_ >= traj.points.size()) { trajectory_active_ = false; as_.setSucceeded(); ROS_INFO("Trajectory execution complete."); return; } const auto& point = traj.points[current_trajectory_point_]; ros::Duration point_time = point.time_from_start; if ((now - traj.header.stamp) < point_time) return; const std::vector<double>& positions = point.positions; const std::vector<double>& velocities = point.velocities; const std::vector<double>& accelerations = point.accelerations; std::array<Motor*, 5> motors = {&motor1, &motor2, &motor3, &motor4, &motor5}; std::array<int, 5> motor_ids = {1, 2, 3, 4, 5}; // 电机 ID { std::lock_guard<std::mutex> lock(sdk_mtx_); for (size_t i = 0; i < 5; ++i) { double pos = positions[i]; double vel = (i < velocities.size()) ? velocities[i] : 0; double acc = (i < accelerations.size()) ? accelerations[i] : 0; motors[i]->moveToPosition(motor_ids[i], static_cast<int>(pos), static_cast<int>(vel), static_cast<int>(acc), static_cast<int>(acc), 1, true, false); } } ROS_INFO_STREAM("Sent trajectory point " << current_trajectory_point_); current_trajectory_point_++; } FollowJointTrajectoryAction::FollowJointTrajectoryAction(const std::string& name) : motor1(1, 7929856), motor2(2, 7929856), motor3(3, 5308416), motor4(4, 5308416), motor5(5, 5308416), as_(nh_, name, false), action_name_(name) { enableMotors(); as_.registerGoalCallback(boost::bind(&FollowJointTrajectoryAction::goalCB, this)); as_.registerPreemptCallback(boost::bind(&FollowJointTrajectoryAction::preemptCB, this)); as_.start(); { std::lock_guard<std::mutex> lk(sdk_mtx_); setupTPDOListeners(); } joint_pub_ = nh_.advertise<sensor_msgs::JointState>("/joint_states", 10); joint_timer_ = nh_.createWallTimer(ros::WallDuration(0.02), &FollowJointTrajectoryAction::publishJointStatesCb, this); reader_running_ = true; reader_thread_ = std::thread(&FollowJointTrajectoryAction::readerLoop, this); } FollowJointTrajectoryAction::~FollowJointTrajectoryAction() { reader_running_ = false; if (reader_thread_.joinable()) { reader_thread_.join(); } } void FollowJointTrajectoryAction::goalCB() { executeTrajectory(as_.acceptNewGoal()); } void FollowJointTrajectoryAction::preemptCB() { ROS_WARN("%s: Preempted", action_name_.c_str()); trajectory_active_ = false; as_.setPreempted(); } void FollowJointTrajectoryAction::publishJointStatesCb(const ros::WallTimerEvent&) { sensor_msgs::JointState joint_state_msg; joint_state_msg.header.stamp = ros::Time::now(); // 补上时间戳 joint_state_msg.name = joint_names_; auto positions = getCurrentPositions(); // 获取当前的位置 joint_state_msg.position.assign(positions.begin(), positions.end()); // 显式转换 joint_pub_.publish(joint_state_msg); // 发布关节状态 } 这是我的一个用于moveit控制五轴机械臂实时运动的action server代码,目前我需要对这个代码进行修改,但是我C代码能力有限,需要你帮助我完成! 以下是我的要求: 1.首先我已经设置了所有电机进入了CSP模式,并且同步周期和插补周期都设置为1ms,那么我现在需要实现的是,目标位置和位置反馈的同步执行,上述代码里的void FollowJointTrajectoryAction::trajectoryTimerCallback函数已经通过SDK实现了自动发送sync帧,但是位置反馈还是异步传输!所以需要改为和位置下发相同周期的同步传输 2.moveit下发的轨迹并不严格匹配插补周期1ms,需要进行插补以实现严格同步,我这里有一个插补算法的实现函数,需要的话我后续可以提供给你 就是这两个核心问题!
10-01
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值