1.让小车直行
rostopic pub /cmd_vel geometry_msgs/Twist "linear:
x: 1.0
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.0"
小车沿着x轴运动
2. 让小车转圈
rostopic pub /cmd_vel geometry_msgs/Twist "linear:
x: 1.0
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 2.0"
小车做圆周运动
odometry1.cpp文件
#include
#include
#include
#include
#include
using namespace std;
int main(int argc,char** argv)
{
ros::init(argc, argv, "cmdveltest");
ros::NodeHandle cmdh;
ros::Publisher cmdpub= cmdh.advertise<geometry_msgs::Twist>("/cmd_vel", 1, true);;
ros::Rate r(60);
while(1){
geometry_msgs::Twist twist;
geometry_msgs::Vector3 linear;
linear.x=0.1;
linear.y=0;
linear.z=0;
geometry_msgs::Vector3 angular;
angular.x=0;
angular.y=0;
//直行
//angular.z=0;
//转圈
angular.z=-0.5;
twist.linear=linear;
twist.angular=angular;
cmdpub.publish(twist);
cout<<"hello"<<endl;
// ros::spinOnce();
//r.sleep();
sleep(1);
}
return 0;
}
在CMakeLists.txt
加入
add_executable(odometry1 src/odometry1.cpp)
target_link_libraries(odometry1 ${catkin_LIBRARIES})
rosrun yuan odometry1 小车做圆形运动
dingdian.py文件
#!/usr/bin/python3
import rospy
import math
import nav_msgs.msg
import geometry_msgs.msg
from tf.transformations import euler_from_quaternion
x = 0
y = 0
w_o = 0
x_o = 0
y_o = 0
z_o = 0
yaw_t = 0
liner_speed = 0
angular_speed = 0
liner_speed_old = 0
angular_speed_old = 0
X_t = 0
Y_t = 0
X_sim = cos(angle)
Y_sim = sin(angle)
def Trans_robot_pose(msg):
global x
global y
global w_o
global x_o
global y_o
global z_o
x = msg.pose.pose.position.x
y = msg.pose.pose.position.y
w_o = msg.pose.pose.orientation.w
x_o = msg.pose.pose.orientation.x
y_o = msg.pose.pose.orientation.y
z_o = msg.pose.pose.orientation.z
return w_o, y_o, z_o, x_o, x, y
if __name__ == '__main__':
rospy.init_node('item1')
turtle_vel = rospy.Publisher('/cmd_vel', geometry_msgs.msg.Twist, queue_size=1)
rate = rospy.Rate(10.0)
while not rospy.is_shutdown():
msg = geometry_msgs.msg.Twist()
(roll, pitch, yaw) = euler_from_quaternion([x_o,y_o,z_o,w_o])
if yaw < 0:
yaw = yaw + 2 * math.pi
X_t = X_sim
Y_t = Y_sim
D_err = math.sqrt(math.pow((X_t - x), 2) + math.pow((Y_t - y), 2))
if (Y_t - y) == 0 and (X_t - x) > 0:
yaw_t = 0
if (Y_t - y) > 0 and (X_t - x) > 0:
yaw_t = math.atan((Y_t - y) / (X_t - x))
if (Y_t - y) > 0 and (X_t - x) == 0:
yaw_t = 0.5 * math.pi
if (Y_t - y) > 0 and (X_t - x) < 0:
yaw_t = math.atan((Y_t - y) / (X_t - x)) + math.pi
if (Y_t - y) == 0 and (X_t - x) < 0:
yaw_t = math.pi
if (Y_t - y) < 0 and (X_t - x) < 0:
yaw_t = math.atan((Y_t - y) / (X_t - x)) + math.pi
if (Y_t - y) < 0 and (X_t - x) == 0:
yaw_t = 1.5 * math.pi
if (Y_t - y) < 0 and (X_t - x) > 0:
yaw_t = math.atan((Y_t - y) / (X_t - x)) + 2 * math.pi
Theta_err = yaw_t - yaw
if Theta_err < -math.pi:
Theta_err = Theta_err + 2 * math.pi
if Theta_err > math.pi:
Theta_err = Theta_err - 2 * math.pi
liner_speed = 0.1 * D_err
angular_speed = 0.7 * Theta_err
msg.linear.x = liner_speed
msg.angular.z = angular_speed
liner_speed_old = liner_speed
angular_speed_old = angular_speed
turtle_vel.publish(msg)
rospy.Subscriber('/odom', nav_msgs.msg.Odometry, Trans_robot_pose)
rate.sleep()
rospy.spin()
在创建功能包时 需要包含文件中需要的所有依赖 否则编译时报错
rosrun yuan dingdian.py
小车会运动到指定地点
guiji.py文件
#!/usr/bin/python3
import rospy
import math
import nav_msgs.msg
import geometry_msgs.msg
from tf.transformations import euler_from_quaternion
x = 0
y = 0
w_o = 0
x_o = 0
y_o = 0
z_o = 0
yaw_t = 0
liner_speed = 0
angular_speed = 0
liner_speed_old = 0
angular_speed_old = 0
X_t = 0
Y_t = 0
X_t_Pre = 0
Y_t_Pre = 0
X_sim = [5, 1, 2.5, 4, 0, 0, 5, 5, 0, 0]
Y_sim = [0, -4, 2.5, -4, 0, 2.5, 2.5, -4, -4, 0]
r = 0
def Trans_robot_pose(msg):
global x
global y
global w_o
global x_o
global y_o
global z_o
x = msg.pose.pose.position.x
y = msg.pose.pose.position.y
w_o = msg.pose.pose.orientation.w
x_o = msg.pose.pose.orientation.x
y_o = msg.pose.pose.orientation.y
z_o = msg.pose.pose.orientation.z
return w_o, y_o, z_o, x_o, x, y
if __name__ == '__main__':
rospy.init_node('item1')
turtle_vel = rospy.Publisher('/cmd_vel', geometry_msgs.msg.Twist, queue_size=1)
rate = rospy.Rate(10.0)
while not rospy.is_shutdown():
msg = geometry_msgs.msg.Twist()
(roll, pitch, yaw) = euler_from_quaternion([x_o,y_o,z_o,w_o])
if yaw < 0:
yaw = yaw + 2 * math.pi
X_t = X_sim[r]
Y_t = Y_sim[r]
D_err = math.sqrt(math.pow((X_t - x), 2) + math.pow((Y_t - y), 2))
if (Y_t - y) == 0 and (X_t - x) > 0:
yaw_t = 0
if (Y_t - y) > 0 and (X_t - x) > 0:
yaw_t = math.atan((Y_t - y) / (X_t - x))
if (Y_t - y) > 0 and (X_t - x) == 0:
yaw_t = 0.5 * math.pi
if (Y_t - y) > 0 and (X_t - x) < 0:
yaw_t = math.atan((Y_t - y) / (X_t - x)) + math.pi
if (Y_t - y) == 0 and (X_t - x) < 0:
yaw_t = math.pi
if (Y_t - y) < 0 and (X_t - x) < 0:
yaw_t = math.atan((Y_t - y) / (X_t - x)) + math.pi
if (Y_t - y) < 0 and (X_t - x) == 0:
yaw_t = 1.5 * math.pi
if (Y_t - y) < 0 and (X_t - x) > 0:
yaw_t = math.atan((Y_t - y) / (X_t - x)) + 2 * math.pi
Theta_err = yaw_t - yaw
if Theta_err < -math.pi:
Theta_err = Theta_err + 2 * math.pi
if Theta_err > math.pi:
Theta_err = Theta_err - 2 * math.pi
if D_err < 0.3:
X_t_Pre = X_t
Y_t_Pre = Y_t
r = r + 1
print r
if r == 10:
r = 0
liner_speed = 0.1 * D_err
angular_speed = 0.7 * Theta_err
msg.linear.x = liner_speed
msg.angular.z = angular_speed
liner_speed_old = liner_speed
angular_speed_old = angular_speed
turtle_vel.publish(msg)
rospy.Subscriber('/odom', nav_msgs.msg.Odometry, Trans_robot_pose)
rate.sleep()
rospy.spin()
rosrun yuan guiji.py
小车沿着轨迹运动
发布nav_msgs/Path类型的消息,然后在rviz上订阅该消息就可以显示轨迹路径
showpath.cpp
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
main(int argc, char **argv)
{
ros::init(argc, argv, "showpath");
ros::NodeHandle ph;
ros::Publisher path_pub = ph.advertise<nav_msgs::Path>("trajectory", 1, true);
ros::Publisher point_pub = ph.advertise<geometry_msgs::PointStamped>("point", 1, true);
ros::Time current_time, last_time;
current_time = ros::Time::now();
last_time = ros::Time::now();
nav_msgs::Path path;
//nav_msgs::Path path;
path.header.stamp = current_time;
path.header.frame_id = "odom";
double x = 0.0;
double y = 0.0;
double z = 0.0;
double th = 0.0;
double vx = 0.1 + 0.2 * rand() / double(RAND_MAX);
double vy = -0.1 + 0.2 * rand() / double(RAND_MAX);
double vth = 0.1;
ros::Rate loop_rate(10);
while (ros::ok())
{
current_time = ros::Time::now();
//compute odometry in a typical way given the velocities of the robot
double dt = 1;
double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
double delta_th = vth * dt;
x += delta_x;
y += delta_y;
z += 0.005;
th += delta_th;
geometry_msgs::PoseStamped this_pose_stamped;
geometry_msgs::PointStamped this_point_stamped;
this_pose_stamped.pose.position.x = x;
this_pose_stamped.pose.position.y = y;
this_pose_stamped.pose.position.z = z;
this_point_stamped.header.stamp = current_time;
this_point_stamped.header.frame_id = "odom";
this_point_stamped.point.x = x;
this_point_stamped.point.y = y;
this_point_stamped.point.z = z;
ROS_INFO("current_x: %f", x);
ROS_INFO("current_y: %f", y);
geometry_msgs::Quaternion goal_quat = tf::createQuaternionMsgFromYaw(th);
this_pose_stamped.pose.orientation.x = goal_quat.x;
this_pose_stamped.pose.orientation.y = goal_quat.y;
this_pose_stamped.pose.orientation.z = goal_quat.z;
this_pose_stamped.pose.orientation.w = goal_quat.w;
this_pose_stamped.header.stamp = current_time;
this_pose_stamped.header.frame_id = "odom";
path.poses.push_back(this_pose_stamped);
path_pub.publish(path);
point_pub.publish(this_point_stamped);
ros::spinOnce(); // check for incoming messages
last_time = current_time;
loop_rate.sleep();
}
return 0;
}
运行showpath.cpp
打开rviz
add path
修改frame_id的odom
问题?
启动gazebo控制小车运动 rviz中没有轨迹显示
rviz中警告
小车发送位置信息需要在模型中添加插件
<plugin name="p3d_base_controller" filename="libgazebo_ros_p3d.so">
<alwaysOn>true</alwaysOn>
<updateRate>50.0</updateRate>
<bodyName>base_link</bodyName>
<topicName>base_pose_ground_truth</topicName>
<gaussianNoise>0.01</gaussianNoise>
<frameName>world</frameName>
<xyzOffsets>0 0 0</xyzOffsets>
<rpyOffsets>0 0 0</rpyOffsets>
</plugin>
监听广播器的launch文件
<launch>
<node pkg="learning_tf" type="tf_broadcaster" args="/turtle1" name="turtle1_tf_broadcaster" />
<node pkg="learning_tf" type="tf_broadcaster" args="/turtle2" name="turtle2_tf_broadcaster" />
<node pkg="learning_tf" type="slave_tf_listener" name="listener" />
</launch>
运行监听广播器后报错
ROS_NAMESPACE=turtle1 rosrun teleop_twist_keyboard teleop_twist_keyboard.py
控制小车1 小车2 不动