# ROS 1.0 melodic or ROS 2.0 Dashing
echo Hello, ROS 1.0 or ROS 2.0? 1=Melodic 2=Dashing 
read ROS
if (($ROS==1));then
source /opt/ros/melodic/setup.bash
#export ROS_PACKAGE_PATH=/home/relaybot/RobTool/ROS1/Wiki/src:/home/relaybot/RobTool/Cozmo/ros/src:$ROS_PACKAGE_PATH
#source /home/relaybot/RobTool/ROS1/Wiki/devel/setup.bash
#export ROS_MASTER_URI=http://192.168.1.100:11311
#export ROS_IP=192.168.1.100
echo "Melodic"
elif (($ROS==2));then
source /opt/ros/dashing/setup.bash
echo "Dashing"
else
echo "Non-ROS"
fi

# turtlebot3 model env TURTLEBOT3_MODEL  model type [burger, waffle, waffle_pi]" name="model"
export TURTLEBOT3_MODEL=waffle

// These constants won't change. They're used to give names to the pins used:
int i=0,j=0,stime=32,sloop=0;
const int atime=16;
const int analogInPin = A0;  // Analog input pin that the potentiometer is attached to

int sensorValue = 0;        // value read from the pot
int outputValue = 0;        // value output to the PWM (analog out)

void setup() {
  // initialize serial communications at 9600 bps:
  pinMode(LED_BUILTIN, OUTPUT);  
  Serial.begin(9600);
}

void loop() {
  // read the analog in value:
  sensorValue = analogRead(analogInPin);
  // map it to the range of the analog out:
  outputValue = map(sensorValue, 0, 1023, 0, atime);
  // change the analog out value:
  stime=outputValue;
  // print the results to the Serial Monitor:
  //Serial.print("sensor = ");
  //Serial.print(sensorValue);
  //Serial.print("\t output = ");
  //Serial.println(outputValue);
  ledfade(stime);
  // wait 2 milliseconds before the next loop for the analog-to-digital
  // converter to settle after the last reading:
}

void ledfade(int i)
{
      digitalWrite(LED_BUILTIN, HIGH);   // turn the LED on (HIGH is the voltage level)
      delay(i);                          // wait for a second
      digitalWrite(LED_BUILTIN, LOW);    // turn the LED off by making the voltage LOW
      delay(atime-i);                    // wait for a second
}

#if (ARDUINO >= 100)
 #include <Arduino.h>
#else
 #include <WProgram.h>
#endif
#include <ros.h>
#include <rosserial_arduino/Adc.h>
#include <geometry_msgs/Twist.h>

ros::NodeHandle nh;

rosserial_arduino::Adc adc_msg;
geometry_msgs::Twist twist_msg;
ros::Publisher p("adc", &adc_msg);
ros::Publisher t("cmd_vel", &twist_msg); 
//turtlesim turtle1/cmd_vel
//turtlebot3 cmd_vel
//husky cmd_vel
void setup()
{ 
  pinMode(13, OUTPUT);
  nh.initNode();
  nh.advertise(p);
  nh.advertise(t);
}

//We average the analog reading to elminate some of the noise
int averageAnalog(int pin){
  int v=0;
  for(int i=0; i<4; i++) v+= analogRead(pin);
  return v/4;
}

long adc_timer;
int x_mid=600,y_mid=600,x_now=600,y_now=600;
int adc_init=100;
void loop()
{
  adc_msg.adc0 = averageAnalog(0);
  adc_msg.adc1 = averageAnalog(1);
  adc_msg.adc2 = averageAnalog(2);
  adc_msg.adc3 = averageAnalog(3);
  adc_msg.adc4 = averageAnalog(4);
  adc_msg.adc5 = averageAnalog(5);
  x_now=adc_msg.adc5-x_mid;
  y_now=adc_msg.adc4-y_mid;
  adc_init=adc_msg.adc3;
  if(adc_init<10){
    x_mid=adc_msg.adc5;
    y_mid=adc_msg.adc4;
  }
  if(x_now>0)
    twist_msg.linear.x=map(x_now,0,1023-x_mid,0,255);
  else
    twist_msg.linear.x=map(x_now,-x_mid,0,-255,0);
  if(y_now>0)
    twist_msg.angular.z=map(y_now,0,1023-y_mid,0,255);
  else
    twist_msg.angular.z=map(y_now,-y_mid,0,-255,0); 

  twist_msg.linear.x=twist_msg.linear.x/(1024.0-adc_msg.adc0)/4.0;
  twist_msg.angular.z=-twist_msg.angular.z/(1024.0-adc_msg.adc0);
         
  p.publish(&adc_msg);
  t.publish(&twist_msg);
  nh.spinOnce();
//  delay(1);
}

roscore

rosrun turtlesim turtlesim_node

rosrun turtle_actionlib shape_server

rosrun turtle_actionlib shape_client

rqt_graph

roslaunch turtlebot3_gazebo turtlebot3_world.launch

rqt_image_view

ros2 run turtlesim turtlesim_node

ros2 run turtlesim turtle_teleop_key

rqt

ros2 launch dummy_robot_bringup dummy_robot_bringup.launch.py

rviz2

Panels:
  - Class: rviz_common/Displays
    Help Height: 78
    Name: Displays
    Property Tree Widget:
      Expanded:
        - /Global Options1
        - /Status1
      Splitter Ratio: 0.5
    Tree Height: 435
  - Class: rviz_common/Selection
    Name: Selection
  - Class: rviz_common/Tool Properties
    Expanded:
      - /2D Nav Goal1
      - /Publish Point1
    Name: Tool Properties
    Splitter Ratio: 0.5886790156364441
  - Class: rviz_common/Views
    Expanded:
      - /Current View1
    Name: Views
    Splitter Ratio: 0.5
Visualization Manager:
  Class: ""
  Displays:
    - Alpha: 0.5
      Cell Size: 1
      Class: rviz_default_plugins/Grid
      Color: 160; 160; 164
      Enabled: true
      Line Style:
        Line Width: 0.029999999329447746
        Value: Lines
      Name: Grid
      Normal Cell Count: 0
      Offset:
        X: 0
        Y: 0
        Z: 0
      Plane: XY
      Plane Cell Count: 10
      Reference Frame: <Fixed Frame>
      Value: true
    - Alpha: 0.699999988079071
      Class: rviz_default_plugins/Map
      Color Scheme: map
      Draw Behind: false
      Enabled: true
      Name: Map
      Topic: /map
      Unreliable: false
      Use Timestamp: false
      Value: true
    - Alpha: 1
      Autocompute Intensity Bounds: true
      Autocompute Value Bounds:
        Max Value: 10
        Min Value: -10
        Value: true
      Axis: Z
      Channel Name: intensity
      Class: rviz_default_plugins/LaserScan
      Color: 255; 255; 255
      Color Transformer: Intensity
      Decay Time: 0
      Enabled: true
      Invert Rainbow: false
      Max Color: 255; 255; 255
      Max Intensity: 4096
      Min Color: 0; 0; 0
      Min Intensity: 0
      Name: LaserScan
      Position Transformer: XYZ
      Queue Size: 10
      Selectable: true
      Size (Pixels): 3
      Size (m): 0.009999999776482582
      Style: Flat Squares
      Topic: /scan
      Unreliable: false
      Use Fixed Frame: true
      Use rainbow: true
      Value: true
    - Alpha: 1
      Axes Length: 1
      Axes Radius: 0.10000000149011612
      Class: rviz_default_plugins/Pose
      Color: 255; 25; 0
      Enabled: true
      Head Length: 0.30000001192092896
      Head Radius: 0.10000000149011612
      Name: Pose
      Shaft Length: 1
      Shaft Radius: 0.05000000074505806
      Shape: Arrow
      Topic: /move_base_simple/goal
      Unreliable: false
      Value: true
    - Class: rviz_default_plugins/TF
      Enabled: true
      Frame Timeout: 15
      Frames:
        All Enabled: true
      Marker Scale: 1
      Name: TF
      Show Arrows: true
      Show Axes: true
      Show Names: false
      Tree:
        {}
      Update Interval: 0
      Value: true
    - Alpha: 1
      Class: rviz_default_plugins/RobotModel
      Collision Enabled: false
      Description File: ""
      Description Source: Topic
      Description Topic: ""
      Enabled: true
      Links:
        All Links Enabled: true
        Expand Joint Details: false
        Expand Link Details: false
        Expand Tree: false
        Link Tree Style: ""
      Name: RobotModel
      TF Prefix: ""
      Unreliable: false
      Update Interval: 0
      Value: true
      Visual Enabled: true
  Enabled: true
  Global Options:
    Background Color: 48; 48; 48
    Fixed Frame: world
    Frame Rate: 30
  Name: root
  Tools:
    - Class: rviz_default_plugins/MoveCamera
    - Class: rviz_default_plugins/Select
    - Class: rviz_default_plugins/FocusCamera
    - Class: rviz_default_plugins/Measure
      Line color: 128; 128; 0
    - Class: rviz_default_plugins/SetInitialPose
      Topic: /initialpose
    - Class: rviz_default_plugins/SetGoal
      Topic: /move_base_simple/goal
    - Class: rviz_default_plugins/PublishPoint
      Single click: true
      Topic: /clicked_point
  Transformation:
    Current:
      Class: rviz_default_plugins/TF
  Value: true
  Views:
    Current:
      Class: rviz_default_plugins/Orbit
      Distance: 8.303964614868164
      Enable Stereo Rendering:
        Stereo Eye Separation: 0.05999999865889549
        Stereo Focal Distance: 1
        Swap Stereo Eyes: false
        Value: false
      Focal Point:
        X: 0.5349314212799072
        Y: -1.8630497455596924
        Z: 0.5543326139450073
      Focal Shape Fixed Size: true
      Focal Shape Size: 0.05000000074505806
      Invert Z Axis: false
      Name: Current View
      Near Clip Distance: 0.009999999776482582
      Pitch: 0.7453981041908264
      Target Frame: <Fixed Frame>
      Value: Orbit (rviz)
      Yaw: 1.8422259092330933
    Saved: ~
Window Geometry:
  Displays:
    collapsed: false
  Height: 664
  Hide Left Dock: false
  Hide Right Dock: true
  QMainWindow State: 000000ff00000000fd0000000400000000000001560000023efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000023e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000023efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000023e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000002a40000023e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
  Selection:
    collapsed: false
  Tool Properties:
    collapsed: false
  Views:
    collapsed: true
  Width: 1024
  X: 0
  Y: 27

ros2 topic pub /demo/cmd_demo geometry_msgs/Twist '{linear: {x: 0.2}}' -1

.
├── Arduino
│   ├── course
│   │   ├── ADCdemo
│   │   ├── AnalogInOutLEDdemo
│   │   ├── AnalogInOutLEDtimedemo
│   │   ├── AnalogInputdemo
│   │   ├── Blinkdemo
│   │   ├── blinkmicrosdemo
│   │   ├── Buttondemo
│   │   ├── Controldemo
│   │   ├── Controlplusdemo
│   │   ├── Controlplusdemo2
│   │   ├── DHTtesterDemo
│   │   ├── dueControldemo
│   │   ├── HCSR04demo
│   │   ├── Knobdemo
│   │   ├── motordemo
│   │   ├── robotdemo
│   │   └── tonePitchFollowerdemo
│   └── libraries
│       ├── Adafruit_Circuit_Playground
│       ├── Adafruit_Unified_Sensor
│       ├── DHT_sensor_library
│       ├── readme.txt
│       ├── Rosserial_Arduino_Library
│       └── SD
├── Desktop
│   └── demo.rviz
├── Documents
│   ├── Arduino
│   │   ├── imutovel.py
│   │   ├── Integrating Arduino-Based Educational Mobile Robots in ROS.pdf
│   │   ├── LabLecture3 Hardware interfacing Arduino.pdf
│   │   ├── note191125.md
│   │   ├── Programming Your First Robot - EE100 Fall 2016.pdf
│   │   └── 机器人控制器编程-总结篇.pdf
│   └── Fritzing
│       ├── bins
│       └── parts
├── Downloads
│   ├── android_camera_imu.apk
│   ├── pinguybuilder_5.1-8_all.deb
│   ├── pycozmo-master.zip
│   └── slambook2-master.zip
├── Music
├── Pictures
│   ├── Screenshot from 2020-01-07 14-44-21.png
│   ├── Screenshot from 2020-01-07 14-51-44.png
│   ├── Screenshot from 2020-01-07 14-55-28.png
│   ├── Screenshot from 2020-01-07 14-56-49.png
│   ├── Screenshot from 2020-01-07 15-05-16.png
│   ├── Screenshot from 2020-01-07 15-18-33.png
│   ├── Screenshot from 2020-01-07 15-54-15.png
│   ├── Screenshot from 2020-01-07 15-57-50.png
│   ├── Screenshot from 2020-01-07 16-08-05.png
│   ├── Screenshot from 2020-01-07 16-26-15.png
│   └── Wallpapers
│       └── ubunturobot.png
├── Public
├── Roft
│   ├── arduino
│   │   └── arduino-1.8.10
│   ├── cozmo_ros2_ws
│   │   ├── build
│   │   ├── install
│   │   ├── log
│   │   └── src
│   ├── Micro-XRCE-DDS-Agent
│   │   ├── agent.refs
│   │   ├── build
│   │   ├── cmake
│   │   ├── CMakeLists.txt
│   │   ├── CTestJenkins.cmake
│   │   ├── Dockerfile
│   │   ├── docs
│   │   ├── include
│   │   ├── LICENSE
│   │   ├── microxrce_agent.cpp
│   │   ├── README.md
│   │   ├── src
│   │   ├── test
│   │   ├── thirdparty
│   │   ├── utils
│   │   └── valgrind.supp
│   ├── opencv-4.1.2
│   │   ├── 3rdparty
│   │   ├── apps
│   │   ├── build
│   │   ├── cmake
│   │   ├── CMakeLists.txt
│   │   ├── CONTRIBUTING.md
│   │   ├── data
│   │   ├── doc
│   │   ├── include
│   │   ├── LICENSE
│   │   ├── modules
│   │   ├── platforms
│   │   ├── README.md
│   │   └── samples
│   └── tello_ros2_ws
│       ├── build
│       ├── install
│       ├── log
│       └── src
├── SLAM
│   └── slambook2
│       ├── ch10
│       ├── ch11
│       ├── ch12
│       ├── ch13
│       ├── ch2
│       ├── ch3
│       ├── ch4
│       ├── ch5
│       ├── ch6
│       ├── ch7
│       ├── ch8
│       ├── ch9
│       ├── errata.cls
│       ├── errata.pdf
│       ├── errata.tex
│       ├── figures
│       ├── LICENSE
│       └── README.md
├── Templates
└── Videos