turtlesim画正方形代码对比

ROS1/2主题/服务/行动基础类turtlesim阶段测试公开题

画正方形或者其他形状有两类实现方式：

1 ☞

#include <boost/bind.hpp>
#include <ros/ros.h>
#include <turtlesim/Pose.h>
#include <geometry_msgs/Twist.h>
#include <std_srvs/Empty.h>turtlesim::PoseConstPtr g_pose;
turtlesim::Pose g_goal;enum State
{FORWARD,STOP_FORWARD,TURN,STOP_TURN,
};State g_state = FORWARD;
State g_last_state = FORWARD;
bool g_first_goal_set = false;#define PI 3.141592void poseCallback(const turtlesim::PoseConstPtr& pose)
{g_pose = pose;
}bool hasReachedGoal()
{return fabsf(g_pose->x - g_goal.x) < 0.1 && fabsf(g_pose->y - g_goal.y) < 0.1 && fabsf(g_pose->theta - g_goal.theta) < 0.01;
}bool hasStopped()
{return g_pose->angular_velocity < 0.0001 && g_pose->linear_velocity < 0.0001;
}void printGoal()
{ROS_INFO("New goal [%f %f, %f]", g_goal.x, g_goal.y, g_goal.theta);
}void commandTurtle(ros::Publisher twist_pub, float linear, float angular)
{geometry_msgs::Twist twist;twist.linear.x = linear;twist.angular.z = angular;twist_pub.publish(twist);
}void stopForward(ros::Publisher twist_pub)
{if (hasStopped()){ROS_INFO("Reached goal");g_state = TURN;g_goal.x = g_pose->x;g_goal.y = g_pose->y;g_goal.theta = fmod(g_pose->theta + PI/2.0, 2*PI);// wrap g_goal.theta to [-pi, pi)if (g_goal.theta >= PI) g_goal.theta -= 2 * PI;printGoal();}else{commandTurtle(twist_pub, 0, 0);}
}void stopTurn(ros::Publisher twist_pub)
{if (hasStopped()){ROS_INFO("Reached goal");g_state = FORWARD;g_goal.x = cos(g_pose->theta) * 2 + g_pose->x;g_goal.y = sin(g_pose->theta) * 2 + g_pose->y;g_goal.theta = g_pose->theta;printGoal();}else{commandTurtle(twist_pub, 0, 0);}
}void forward(ros::Publisher twist_pub)
{if (hasReachedGoal()){g_state = STOP_FORWARD;commandTurtle(twist_pub, 0, 0);}else{commandTurtle(twist_pub, 1.0, 0.0);}
}void turn(ros::Publisher twist_pub)
{if (hasReachedGoal()){g_state = STOP_TURN;commandTurtle(twist_pub, 0, 0);}else{commandTurtle(twist_pub, 0.0, 0.4);}
}void timerCallback(const ros::TimerEvent&, ros::Publisher twist_pub)
{if (!g_pose){return;}if (!g_first_goal_set){g_first_goal_set = true;g_state = FORWARD;g_goal.x = cos(g_pose->theta) * 2 + g_pose->x;g_goal.y = sin(g_pose->theta) * 2 + g_pose->y;g_goal.theta = g_pose->theta;printGoal();}if (g_state == FORWARD){forward(twist_pub);}else if (g_state == STOP_FORWARD){stopForward(twist_pub);}else if (g_state == TURN){turn(twist_pub);}else if (g_state == STOP_TURN){stopTurn(twist_pub);}
}int main(int argc, char** argv)
{ros::init(argc, argv, "draw_square");ros::NodeHandle nh;ros::Subscriber pose_sub = nh.subscribe("turtle1/pose", 1, poseCallback);ros::Publisher twist_pub = nh.advertise<geometry_msgs::Twist>("turtle1/cmd_vel", 1);ros::ServiceClient reset = nh.serviceClient<std_srvs::Empty>("reset");ros::Timer timer = nh.createTimer(ros::Duration(0.016), boost::bind(timerCallback, _1, twist_pub));std_srvs::Empty empty;reset.call(empty);ros::spin();
}

2 ☞

#include <ros/ros.h>
#include <turtlesim/Pose.h>
#include <actionlib/server/simple_action_server.h>
#include <cmath>
#include <math.h>
#include <angles/angles.h>#include <geometry_msgs/Twist.h>
#include <turtle_actionlib/ShapeAction.h>// This class computes the command_velocities of the turtle to draw regular polygons
class ShapeAction
{
public:ShapeAction(std::string name) : as_(nh_, name, false),action_name_(name){//register the goal and feeback callbacksas_.registerGoalCallback(boost::bind(&ShapeAction::goalCB, this));as_.registerPreemptCallback(boost::bind(&ShapeAction::preemptCB, this));//subscribe to the data topic of interestsub_ = nh_.subscribe("/turtle1/pose", 1, &ShapeAction::controlCB, this);pub_ = nh_.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 1);as_.start();}~ShapeAction(void){}void goalCB(){// accept the new goalturtle_actionlib::ShapeGoal goal = *as_.acceptNewGoal();//save the goal as private variablesedges_ = goal.edges;radius_ = goal.radius;// reset helper variablesinterior_angle_ = ((edges_-2)*M_PI)/edges_;apothem_ = radius_*cos(M_PI/edges_);//compute the side length of the polygonside_len_ = apothem_ * 2* tan( M_PI/edges_);//store the result valuesresult_.apothem = apothem_;result_.interior_angle = interior_angle_;edge_progress_ =0;start_edge_ = true;}void preemptCB(){ROS_INFO("%s: Preempted", action_name_.c_str());// set the action state to preemptedas_.setPreempted();}void controlCB(const turtlesim::Pose::ConstPtr& msg){// make sure that the action hasn't been canceledif (!as_.isActive())return;if (edge_progress_ < edges_){// scalar values for drive the turtle faster and straighterdouble l_scale = 6.0;double a_scale = 6.0;double error_tol = 0.00001;if (start_edge_){start_x_ = msg->x;start_y_ = msg->y;start_theta_ = msg->theta;start_edge_ = false;}// compute the distance and theta error for the shapedis_error_ = side_len_ - fabs(sqrt((start_x_- msg->x)*(start_x_-msg->x) + (start_y_-msg->y)*(start_y_-msg->y)));theta_error_ = angles::normalize_angle_positive(M_PI - interior_angle_ - (msg->theta - start_theta_));if (dis_error_ > error_tol){command_.linear.x = l_scale*dis_error_;command_.angular.z = 0;}else if (dis_error_ < error_tol && fabs(theta_error_)> error_tol){ command_.linear.x = 0;command_.angular.z = a_scale*theta_error_;}else if (dis_error_ < error_tol && fabs(theta_error_)< error_tol){command_.linear.x = 0;command_.angular.z = 0;start_edge_ = true;edge_progress_++;}  else{command_.linear.x = l_scale*dis_error_;command_.angular.z = a_scale*theta_error_;} // publish the velocity commandpub_.publish(command_);} else{          ROS_INFO("%s: Succeeded", action_name_.c_str());// set the action state to succeededas_.setSucceeded(result_);}   }protected:ros::NodeHandle nh_;actionlib::SimpleActionServer<turtle_actionlib::ShapeAction> as_;std::string action_name_;double radius_, apothem_, interior_angle_, side_len_;double start_x_, start_y_, start_theta_;double dis_error_, theta_error_;int edges_ , edge_progress_;bool start_edge_;geometry_msgs::Twist command_;turtle_actionlib::ShapeResult result_;ros::Subscriber sub_;ros::Publisher pub_;
};int main(int argc, char** argv)
{ros::init(argc, argv, "turtle_shape");ShapeAction shape(ros::this_node::getName());ros::spin();return 0;
}

#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <actionlib/client/terminal_state.h>
#include <turtle_actionlib/ShapeAction.h>int main (int argc, char **argv)
{ros::init(argc, argv, "test_shape"); // create the action client// true causes the client to spin it's own threadactionlib::SimpleActionClient<turtle_actionlib::ShapeAction> ac("turtle_shape", true); ROS_INFO("Waiting for action server to start.");// wait for the action server to startac.waitForServer(); //will wait for infinite timeROS_INFO("Action server started, sending goal.");// send a goal to the action turtle_actionlib::ShapeGoal goal;goal.edges = 4;goal.radius = 1.3;ac.sendGoal(goal);//wait for the action to returnbool finished_before_timeout = ac.waitForResult(ros::Duration(40.0));if (finished_before_timeout){actionlib::SimpleClientGoalState state = ac.getState();ROS_INFO("Action finished: %s",state.toString().c_str());}else  ROS_INFO("Action did not finish before the time out.");//exitreturn 0;
}

对比发现第2种，精度高一些，为何？

如何提升第1种方式绘制图形的精度。

如何绘制任意图形？

turtlesim之欢度国庆（ROS2二维轨迹之随心所欲）

turtlesim画正方形代码对比相关推荐

画正方形代码（c++）
我看了一下c++的练习书上有一个让我们打印出一个正方形好了上代码! (跪求赞和评论!!) (跪求赞和评论!!) (跪求赞和评论!!) #include<bits/stdc++.h> ...
canvas画正方形、三角形以及虚线
1.画正方形代码如下 <style>canvas {margin: 0 auto;border: 2px solid #aaa;display: block; /*画布居中*/} < ...
python画正方形的代码_python画正方形的代码是什么？
python画正方形的代码是什么? python画正方形的代码是:import turtle #导入 turtle.title("画正方形") turtle.pensize(5) ...
用python的turtle画正方形内切圆_Python 用turtle实现用正方形画圆的例子
最近发现一个很有意思的画图的python库,叫做turtle,这里先说下用turtle这个库来实现用正方形画圆的思路. 每次都用乌龟(turtle) 来画出一个正方形,然后通过旋转3°后,继续画一样的 ...
python画正方形-用python画正方形
广告关闭腾讯云11.11云上盛惠 ,精选热门产品助力上云,云服务器首年88元起,买的越多返的越多,最高返5000元! 我正在学习python学习教程,我无法打开屏幕进行绘图. 我没有发现错误,它只是 ...
python画正方形并涂色_这种图片怎么用python画出来，每一个数字代表一种颜色?...
Python 画正方形,写数字的代码如下: from PIL import Image, ImageDraw, ImageFont, ImageFilter WIDTH = 100 * 4 HEIGH ...
opengl入门基础-画正方形
提示:文章写完后,目录可以自动生成,如何生成可参考右边的帮助文档文章目录一.运行环境二.opengl画正方形三.步骤代码下载一.运行环境 windows visual studio 291 ...
vue3.0 + typescript openlayers实现地图标点、移动、画线、显示范围、测量长度、测量面积、画三角形、画正方形、画圆、线选、画笔、清除测量、清除、地图上展示弹窗等功能
vue3.0 + typescript openlayers实现地图标点.移动.画线.显示范围.测量长度.测量面积.画三角形.画正方形.画圆.线选.画笔.清除测量.清除地图所有等功能由于最近项目中用 ...
python 绘制折线图显示纵坐标值_python怎样用matplotlib画折线图对比图
在论文写作中经常会出现多个算法效果比较的折线对比图,本文就介绍一下,怎样通过python中的matplotlib库来画出折线对比图. 工具/原料 python matploblib 方法/步骤 1 M ...

turtlesim画正方形代码对比

turtlesim画正方形代码对比相关推荐

最新文章

热门文章