ROS1 Gazebo Orbits

ros1_gazebo_magnet.ipynb

ros1_gazebo_orbits.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import rospy  \n",
    "\n",
    "# Give ourselves the ability to run a dynamic reconfigure server.\n",
    "from dynamic_reconfigure.server import Server as DynamicReconfigureServer\n",
    "# from node_example.cfg import nodeExampleConfig as ConfigType\n",
    "\n",
    "# Import gazebo message data types.\n",
    "from gazebo_msgs.msg import ModelState, ModelStates"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "class NodeExample(object):\n",
    "    \"\"\"Node example class.\"\"\"\n",
    "\n",
    "    def __init__(self):\n",
    "        \"\"\"Read in parameters.\"\"\"\n",
    "        # Get the private namespace parameters from the parameter server:\n",
    "        # Initialize from either command line or launch file.\n",
    "        self.enable = rospy.get_param(\"~enable\", True)\n",
    "        self.prefix = rospy.get_param(\"~prefix\", \"unit_sphere\")\n",
    "        self.rate = rospy.get_param(\"~rate\", 1.0)\n",
    "        \n",
    "        # Create a dynamic reconfigure server.\n",
    "#         self.server = DynamicReconfigureServer(ConfigType, self.reconfigure_cb)\n",
    "                \n",
    "        # Create a publisher and subscriber for gazebo.\n",
    "        self.pub = rospy.Publisher(\n",
    "            \"/gazebo/set_model_state\", ModelState,\n",
    "            queue_size=10)\n",
    "        self.sub = rospy.Subscriber(\n",
    "            \"/gazebo/model_states\", ModelStates,\n",
    "            self.orbit_cb, queue_size=10)\n",
    "\n",
    "        if self.enable:\n",
    "            self.start()\n",
    "        else:\n",
    "            self.stop()\n",
    "\n",
    "    def start(self):\n",
    "        \"\"\"Turn on publisher.\"\"\"\n",
    "        self.sub = rospy.Subscriber(\n",
    "            \"/gazebo/model_states\", ModelStates,\n",
    "            self.orbit_cb, queue_size=10)\n",
    "\n",
    "    def stop(self):\n",
    "        \"\"\"Turn off publisher.\"\"\"\n",
    "        self.pub.unregister()\n",
    "\n",
    "    def orbit_cb(self, model_states):\n",
    "        \"\"\"Call at a specified interval to publish message.\"\"\"\n",
    "        if not self.enable:\n",
    "            return\n",
    "        \n",
    "        t = rospy.get_rostime().to_sec()\n",
    "        \n",
    "        # Loop over model states\n",
    "        for model_name, model_pose, model_twist in zip(\n",
    "            model_states.name, model_states.pose, model_states.twist):\n",
    "            # Filter via model names\n",
    "            if model_name.startswith(self.prefix):\n",
    "                model_state = ModelState()\n",
    "                model_state.model_name = model_name\n",
    "                model_state.pose = model_pose\n",
    "#                 model_state.twist = model_twist\n",
    "                \n",
    "                # Compute next pose via clock\n",
    "                radius = np.linalg.norm([\n",
    "                        model_state.pose.position.x,\n",
    "                        model_state.pose.position.y\n",
    "                    ])\n",
    "                model_state.pose.position.x = radius * np.cos(self.rate * t * 2 * np.pi)\n",
    "                model_state.pose.position.y = radius * np.sin(self.rate * t * 2 * np.pi)\n",
    "                \n",
    "                # Publish to set model state\n",
    "                self.pub.publish(model_state)\n",
    "\n",
    "    def reconfigure_cb(self, config, dummy):\n",
    "        \"\"\"Create a callback function for the dynamic reconfigure server.\"\"\"\n",
    "        # Fill in local variables with values received from dynamic reconfigure\n",
    "        # clients (typically the GUI).\n",
    "        self.prefix = config[\"prefix\"]\n",
    "        self.rate = config[\"rate\"]\n",
    "\n",
    "        # Check to see if node should be started or stopped.\n",
    "        if self.enable != config[\"enable\"]:\n",
    "            if config[\"enable\"]:\n",
    "                self.start()\n",
    "            else:\n",
    "                self.stop()\n",
    "        self.enable = config[\"enable\"]\n",
    "\n",
    "        # Return the new variables.\n",
    "        return config"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Main function.\n",
    "def main():\n",
    "    # Initialize the node and name it.\n",
    "    rospy.init_node(\"gazebo_orbits\")\n",
    "    # Go to class functions that do all the heavy lifting.\n",
    "    try:\n",
    "        NodeExample()\n",
    "    except rospy.ROSInterruptException:\n",
    "        pass\n",
    "    # Allow ROS to go to all callbacks.\n",
    "    rospy.spin()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# main()\n",
    "if __name__ == \"__main__\":\n",
    "    main()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "<video src=\"https://i.imgur.com/7eYb5Ue.mp4\" controls  >\n",
       "      Your browser does not support the <code>video</code> element.\n",
       "    </video>"
      ],
      "text/plain": [
       "<IPython.core.display.Video object>"
      ]
     },
     "execution_count": 4,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "from IPython.display import Video\n",
    "Video(\"https://i.imgur.com/7eYb5Ue.mp4\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "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.6.9"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}