Skip to content

Commit

Permalink
solution 5.4: circular trajectory
Browse files Browse the repository at this point in the history
  • Loading branch information
rhaschke committed Nov 18, 2023
1 parent c6a6cca commit c8ccd2d
Showing 1 changed file with 95 additions and 0 deletions.
95 changes: 95 additions & 0 deletions notebook/task5.4.ipynb
Original file line number Diff line number Diff line change
@@ -0,0 +1,95 @@
{
"metadata": {
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.8.5"
},
"orig_nbformat": 2,
"kernelspec": {
"name": "python385jvsc74a57bd031f2aee4e71d21fbe5cf8b01ff0e069b9275f58929596ceb00d14d90e3e16cd6",
"display_name": "Python 3.8.5 64-bit ('usr')"
},
"metadata": {
"interpreter": {
"hash": "31f2aee4e71d21fbe5cf8b01ff0e069b9275f58929596ceb00d14d90e3e16cd6"
}
}
},
"nbformat": 4,
"nbformat_minor": 2,
"cells": [
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"import rospy\n",
"from visualization_msgs.msg import Marker, MarkerArray\n",
"from std_msgs.msg import ColorRGBA\n",
"from geometry_msgs.msg import Point, Quaternion\n",
"from tf import transformations as tf\n",
"import numpy as np\n",
"\n",
"rospy.init_node('circular')\n",
"pub = rospy.Publisher('/marker_array', MarkerArray, queue_size=1)"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"def point(theta, id, radius=1.0, scale=0.01, color=ColorRGBA(0,1,1,1), frame_id='world', ns='circle'):\n",
" m = Marker()\n",
" m.ns = ns\n",
" m.type = Marker.SPHERE\n",
" m.id = id\n",
" m.scale.x = m.scale.y = m.scale.z = scale\n",
" m.color = color\n",
" m.header.frame_id = frame_id\n",
" \n",
" q = tf.quaternion_about_axis(angle=theta, axis=[0,0,1])\n",
" m.pose.orientation = Quaternion(*q)\n",
" m.pose.position = Point(*(tf.quaternion_matrix(q)[:3, :3].dot([1,0,0])))\n",
" return m"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# create static array of points\n",
"points = [point(theta, id=i+10) for i, theta in enumerate(np.arange(0,2*np.pi,0.05))]\n",
"pub.publish(MarkerArray(points))"
]
},
{
"source": [
"# create moving point\n",
"rate = rospy.Rate(10) # 10 Hz\n",
"theta = 0\n",
"color = ColorRGBA(1,0,0,1)\n",
"while True:\n",
" theta += 0.05\n",
" pub.publish(MarkerArray([point(theta, id=9, scale=0.02, color=color, ns='point')]))\n",
" rate.sleep()"
],
"cell_type": "code",
"metadata": {},
"execution_count": null,
"outputs": []
}
]
}

0 comments on commit c8ccd2d

Please # to comment.