Clean up & commenting

docker_env/gazebo/Dockerfile

@@ -3,15 +3,15 @@
 # Use official image for Gazebo 9.x 
 FROM gazebo:gzserver9
 
-# COPY $PWD/gazebo_models /root/.gazebo/models
-
 RUN apt-get update
+# Install packages required for developing with gazebo
 RUN apt-get install -y libgazebo9-dev
 RUN sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
 RUN apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654
 RUN apt-get install -y curl
 RUN curl -sSL 'http://keyserver.ubuntu.com/pks/lookup?op=get&search=0xC1CF6E31E6BADE8868B172B4F42ED6FBAB17C654' | apt-key add -
 RUN apt-get update
+# Install packages required for developing with ROS
 RUN apt-get install -y ros-melodic-desktop-full
 RUN echo "source /opt/ros/melodic/setup.bash" >> ~/.bashrc
 RUN apt install -y python-rosdep
@@ -28,16 +28,12 @@ RUN usermod -aG sudo rrrobot
 WORKDIR /home/rrrobot
 RUN chown -R rrrobot:rrrobot /home/rrrobot
 
-#COPY $RRROBOT_HOME/src /home/rrrobot/src
+# Initialize the environment in .bashrc
-#COPY /home/dwitcpa/eecs467/rrrobot/src /home/rrrobot
-
 RUN echo "source /opt/ros/melodic/setup.bash" >> /home/rrrobot/.bashrc
 RUN echo "source /usr/share/gazebo/setup.sh" >> /home/rrrobot/.bashrc
 RUN echo "export GAZEBO_MODEL_PATH=/home/rrrobot/rrrobot_src/src/gazebo_models:\$GAZEBO_MODEL_PATH" >> /home/rrrobot/.bashrc
 RUN echo "export GAZEBO_PLUGIN_PATH=/home/rrrobot/rrrobot_src/lib:\$GAZEBO_PLUGIN_PATH" >> /home/rrrobot/.bashrc
 
-#RUN chown -R rrrobot:rrrobot /home/rrrobot/.gazebo
-
 USER rrrobot
 
 CMD ["/bin/bash"]

rrrobot_src/src/simulation_drivers/src/simulation_env/src/arm_angle_sensor_plugin.cpp

@@ -1,6 +1,5 @@
 #include <gazebo/gazebo.hh>
 #include <gazebo/physics/physics.hh>
-#include <std_msgs/Float32.h>
 #include <memory>
 #include <iostream>
 
@@ -14,10 +13,10 @@ namespace gazebo
 {
   class JointAngle : public ModelPlugin
   {
-    public: 
+  public: 
     void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
     {
-      cout << "Loading model" << endl;
+      cout << "Loading JointAngle sensor plugin for arm" << endl;
 
       model = _parent;
 
@@ -33,13 +32,12 @@ namespace gazebo
       publisher = nh->advertise<simulation_env::arm_angles>("arm_positions", 1);      
     }
 
-    private:
+  private:
-
     void onUpdate()
     {
-      //cout << __func__ << endl;
       simulation_env::arm_angles msg;
-      //msg.data = 0.0f;
+
+      // read in the joint angle for each joint in the arm
       msg.shoulder_pivot_angle = dynamic_cast<gazebo::physics::Joint*>(model->GetChild("shoulder_pivot").get())->Position();
       msg.shoulder_joint_angle = dynamic_cast<gazebo::physics::Joint*>(model->GetChild("shoulder_joint").get())->Position();
       msg.elbow_joint_angle = dynamic_cast<gazebo::physics::Joint*>(model->GetChild("elbow_joint").get())->Position();
@@ -47,18 +45,13 @@ namespace gazebo
       msg.wrist_joint_angle = dynamic_cast<gazebo::physics::Joint*>(model->GetChild("wrist_joint").get())->Position();
       msg.end_effector_pivot_angle = dynamic_cast<gazebo::physics::Joint*>(model->GetChild("end_effector_pivot").get())->Position();
       
-      //gazebo::physics::BasePtr joint = model->GetChild("shoulder_joint");
+      // publish the updated sensor measurements
-      //cout << "Joint type: " << joint->GetType() << "joint type & EntityType::JOINT = " << (joint->GetType() & gazebo::physics::Base::EntityType::JOINT) << endl;
+      publisher.publish(msg);
-      //if((joint->GetType() & gazebo::physics::Base::EntityType::JOINT) != 0)
+
-      //{
+      // make sure the message gets published
-        //cout << "Got an element of type JOINT" << endl;
-        //cout << "Joint Position: " << dynamic_cast<gazebo::physics::Joint*>(joint.get())->Position() << endl;
-        //msg.data = dynamic_cast<gazebo::physics::Joint*>(joint.get())->Position();
-	//cout << "Position: " << msg.data << endl;
-      //}
-      /*if(msg.data != 0.0f) */publisher.publish(msg);
       ros::spinOnce();
     }
+
     physics::ModelPtr model;
     event::ConnectionPtr updateConnection;
     std::unique_ptr<ros::NodeHandle> nh;

run_rrrobot_image.sh

@@ -1,5 +1,3 @@
-#docker run -i -h rrrobot-env -t eecs467:rrrobot bash
-
 xhost +local:docker #rrrobot-env
 docker run -it --rm --device=/dev/input/event4 --device=/dev/dri:/dev/dri -e DISPLAY=$DISPLAY -v /tmp/.X11-unix:/tmp/.X11-unix -v $PWD/rrrobot_src:/home/rrrobot/rrrobot_src -h rrrobot-env eecs467:rrrobot
 xhost -local:docker #rrrobot-env