CISC 367�Mobile Robot Programming

Instructor: Christopher Rasmussen (cer@cis.udel.edu)

​

​

Course page: http://nameless.cis.udel.edu/class_wiki/index.php/CISC367_S2023

​

February 22, 2023 ❖ Class 5

Feb. 22 robot status check & TEAMS v1.0

zelda Jinay, Matthew, Talha

qbert Jonathan, Ryan, Guru

blinky

yoshi Quinten, Harrison, Luke, Jake

sonic Amani, Justin, Kurt

ROS 2 Nodes & Topics

• A node is a functional element made up of one or more processes
• Topics are channels of communication between nodes
• ros2 topic list to see names of active topics
• ros2 topic list -t to see their types also
• ros2 topic echo <topic name> to see actual messages being sent
• Publishing = Writing to a topic (can be multicast)
• Subscribing = Reading from a topic
• Talker-listener example

ROS 2 Nodes & Topics

• A node is a functional element made up of one or more processes
• Topics are channels of communication between nodes
• ros2 topic list to see names of active topics
• ros2 topic list -t to see their types also
• ros2 topic echo <topic name> to see actual messages being sent
• Publishing = Writing to a topic (can be multicast)
• Subscribing = Reading from a topic
• Talker-listener example

PROGRAMMATICALLY

ROS 2 Workspaces & Packages

• A workspace such as create3_ws or ros2_ws is a directory containing ROS 2 packages that are in addition to main ROS installation
• Packages live in subdirectory src
• A package is a "container" for ROS code (C++ or Python). It can have multiple node definitions, configuration files, launch files, and miscellaneous project-related information
• ros2 pkg create … to make an empty package with package.xml and CMakeLists.txt or setup.py

​

workspace_folder/

src/

package_1/

CMakeLists.txt

package.xml

​

package_2/

setup.py

package.xml

resource/package_2

...

package_n/

CMakeLists.txt

package.xml

Building & Running Packages

• colcon build … from the workspace directory
• First time will create build, install, and log dirs
• Actually attempts to compile packages
• Can select individual package with --packages-select <package name>
• Included/imported files in code can require dependency changes in package.xml, CMakeLists.txt
• Source install/local_setup.bash/.bat after colcon to register newly-built packages
• Start node with

ros2 run <package name> <node name>

​

HW #2

• wanderer: Go straight until bumper says you hit something. When it happens: stop, back up, spin randomly, and keep going

​

​

Create 3 Subscription Topics for wanderer

$ ros2 topic list -t

/battery_state [sensor_msgs/msg/BatteryState]

/cmd_audio [irobot_create_msgs/msg/AudioNoteVector]

/cmd_lightring [irobot_create_msgs/msg/LightringLeds]

/cmd_vel [geometry_msgs/msg/Twist]

/dock [irobot_create_msgs/msg/Dock]

/hazard_detection [irobot_create_msgs/msg/HazardDetectionVector]

/imu [sensor_msgs/msg/Imu]

/interface_buttons [irobot_create_msgs/msg/InterfaceButtons]

/ir_intensity [irobot_create_msgs/msg/IrIntensityVector]

/ir_opcode [irobot_create_msgs/msg/IrOpcode]

/kidnap_status [irobot_create_msgs/msg/KidnapStatus]

/mouse [irobot_create_msgs/msg/Mouse]

/odom [nav_msgs/msg/Odometry]

/parameter_events [rcl_interfaces/msg/ParameterEvent]

/rosout [rcl_interfaces/msg/Log]

/slip_status [irobot_create_msgs/msg/SlipStatus]

/stop_status [irobot_create_msgs/msg/StopStatus]

/tf [tf2_msgs/msg/TFMessage]

/tf_static [tf2_msgs/msg/TFMessage]

/wheel_status [irobot_create_msgs/msg/WheelStatus]

/wheel_ticks [irobot_create_msgs/msg/WheelTicks]

/wheel_vels [irobot_create_msgs/msg/WheelVels]

Create 3 Topics for HW #2 wanderer

HazardDetection message

HazardDetectionVector message

Talker/Listener (C++)

#include <memory>

#include "rclcpp/rclcpp.hpp"

#include "std_msgs/msg/string.hpp"

using std::placeholders::_1;

​

class MinimalSubscriber : public rclcpp::Node

{

public:

MinimalSubscriber() : Node("minimal_subscriber")

{

subscription_ = this->create_subscription<std_msgs::msg::String>(

"topic", 10,

std::bind(&MinimalSubscriber::topic_callback, this, _1));

}

​

private:

void topic_callback(const std_msgs::msg::String & msg) const

{

RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg.data.c_str());

}

​

rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;

};

​

int main(int argc, char * argv[])

{

rclcpp::init(argc, argv);

rclcpp::spin(std::make_shared<MinimalSubscriber>());

rclcpp::shutdown();

return 0;

}

​

Talker/Listener (C++) basic changes

#include <memory>

#include "rclcpp/rclcpp.hpp"

#include "std_msgs/msg/string.hpp"

using std::placeholders::_1;

​

class MinimalSubscriber : public rclcpp::Node

{

public:

MinimalSubscriber() : Node("minimal_subscriber")

{

subscription_ = this->create_subscription<std_msgs::msg::String>(

"topic", 10,

std::bind(&MinimalSubscriber::topic_callback, this, _1));

}

​

private:

void topic_callback(const std_msgs::msg::String & msg) const

{

RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg.data.c_str());

}

​

rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;

};

​

int main(int argc, char * argv[])

{

rclcpp::init(argc, argv);

rclcpp::spin(std::make_shared<MinimalSubscriber>());

rclcpp::shutdown();

return 0;

}

​

Talker/Listener (C++) basic changes

#include <memory>

#include "rclcpp/rclcpp.hpp"

#include "std_msgs/msg/string.hpp"

using std::placeholders::_1;

​

class MinimalSubscriber : public rclcpp::Node

{

public:

MinimalSubscriber() : Node("minimal_subscriber")

{

subscription_ = this->create_subscription<std_msgs::msg::String>(

"topic", 10,

std::bind(&MinimalSubscriber::topic_callback, this, _1));

}

​

private:

void topic_callback(const std_msgs::msg::String & msg) const

{

RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg.data.c_str());

}

​

rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;

};

​

int main(int argc, char * argv[])

{

rclcpp::init(argc, argv);

rclcpp::spin(std::make_shared<MinimalSubscriber>());

rclcpp::shutdown();

return 0;

}

​

watch out for QoS issues here -- I used rclcpp::SensorDataQoS()

Create 3 Publication Topics for wanderer

$ ros2 topic list -t

/battery_state [sensor_msgs/msg/BatteryState]

/cmd_audio [irobot_create_msgs/msg/AudioNoteVector]

/cmd_lightring [irobot_create_msgs/msg/LightringLeds]

/cmd_vel [geometry_msgs/msg/Twist]

/dock [irobot_create_msgs/msg/Dock]

/hazard_detection [irobot_create_msgs/msg/HazardDetectionVector]

/imu [sensor_msgs/msg/Imu]

/interface_buttons [irobot_create_msgs/msg/InterfaceButtons]

/ir_intensity [irobot_create_msgs/msg/IrIntensityVector]

/ir_opcode [irobot_create_msgs/msg/IrOpcode]

/kidnap_status [irobot_create_msgs/msg/KidnapStatus]

/mouse [irobot_create_msgs/msg/Mouse]

/odom [nav_msgs/msg/Odometry]

/parameter_events [rcl_interfaces/msg/ParameterEvent]

/rosout [rcl_interfaces/msg/Log]

/slip_status [irobot_create_msgs/msg/SlipStatus]

/stop_status [irobot_create_msgs/msg/StopStatus]

/tf [tf2_msgs/msg/TFMessage]

/tf_static [tf2_msgs/msg/TFMessage]

/wheel_status [irobot_create_msgs/msg/WheelStatus]

/wheel_ticks [irobot_create_msgs/msg/WheelTicks]

/wheel_vels [irobot_create_msgs/msg/WheelVels]

but for finite durations!!

ROS2 Actions would be nice here!

/audio_note_sequence [irobot_create_msgs/action/AudioNoteSequence]

/dock [irobot_create_msgs/action/DockServo]

/drive_arc [irobot_create_msgs/action/DriveArc]

/drive_distance [irobot_create_msgs/action/DriveDistance]

/led_animation [irobot_create_msgs/action/LedAnimation]

/navigate_to_position [irobot_create_msgs/action/NavigateToPosition]

/rotate_angle [irobot_create_msgs/action/RotateAngle]

/undock [irobot_create_msgs/action/Undock]

/wall_follow [irobot_create_msgs/action/WallFollow]

​

• Longer term commands
• Goal is sent by client and acknowledged by server
• Server sends feedback while working on task
• Server notifies client if/when goal has been reached
• More complicated to interact with programmatically
• We will revisit with nav2 package

​

Create 3 Publication Topics for wanderer

(+x = forward)

(+y = left)

(+z = up)

linear units = m / s

angular units = rad / s (positive → counterclockwise)

Talker/Listener (C++)

​

#include <chrono>

#include <functional>

#include <memory>

#include <string>

​

#include "rclcpp/rclcpp.hpp"

#include "std_msgs/msg/string.hpp"

​

using namespace std::chrono_literals;

​

class MinimalPublisher : public rclcpp::Node

{

public:

MinimalPublisher()

: Node("minimal_publisher"), count_(0)

{

publisher_ = this->create_publisher<std_msgs::msg::String>("topic", 10);

timer_ = this->create_wall_timer(

500ms, std::bind(&MinimalPublisher::timer_callback, this));

}

private:

void timer_callback()

{

auto message = std_msgs::msg::String();

message.data = "Hello, world! " + std::to_string(count_++);

RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str());

publisher_->publish(message);

}

rclcpp::TimerBase::SharedPtr timer_;

rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;

size_t count_;

};

​

int main(int argc, char * argv[])

{

rclcpp::init(argc, argv);

rclcpp::spin(std::make_shared<MinimalPublisher>());

rclcpp::shutdown();

return 0;

}

Talker/Listener (C++) basic changes

​

#include <chrono>

#include <functional>

#include <memory>

#include <string>

​

#include "rclcpp/rclcpp.hpp"

#include "std_msgs/msg/string.hpp"

​

using namespace std::chrono_literals;

​

class MinimalPublisher : public rclcpp::Node

{

public:

MinimalPublisher()

: Node("minimal_publisher"), count_(0)

{

publisher_ = this->create_publisher<std_msgs::msg::String>("topic", 10);

timer_ = this->create_wall_timer(

500ms, std::bind(&MinimalPublisher::timer_callback, this));

}

private:

void timer_callback()

{

auto message = std_msgs::msg::String();

message.data = "Hello, world! " + std::to_string(count_++);

RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str());

publisher_->publish(message);

}

rclcpp::TimerBase::SharedPtr timer_;

rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;

size_t count_;

};

​

int main(int argc, char * argv[])

{

rclcpp::init(argc, argv);

rclcpp::spin(std::make_shared<MinimalPublisher>());

rclcpp::shutdown();

return 0;

}

Talker/Listener (C++) basic changes

​

#include <chrono>

#include <functional>

#include <memory>

#include <string>

​

#include "rclcpp/rclcpp.hpp"

#include "std_msgs/msg/string.hpp"

​

using namespace std::chrono_literals;

​

class MinimalPublisher : public rclcpp::Node

{

public:

MinimalPublisher()

: Node("minimal_publisher"), count_(0)

{

publisher_ = this->create_publisher<std_msgs::msg::String>("topic", 10);

timer_ = this->create_wall_timer(

500ms, std::bind(&MinimalPublisher::timer_callback, this));

}

private:

void timer_callback()

{

auto message = std_msgs::msg::String();

message.data = "Hello, world! " + std::to_string(count_++);

RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str());

publisher_->publish(message);

}

rclcpp::TimerBase::SharedPtr timer_;

rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;

size_t count_;

};

​

int main(int argc, char * argv[])

{

rclcpp::init(argc, argv);

rclcpp::spin(std::make_shared<MinimalPublisher>());

rclcpp::shutdown();

return 0;

}

The key to wanderer is understanding this function and how to use it. Default behavior is repeat

HW #2

• reactor: Use infrared range sensors to reactively avoid obstacles before you hit them

​

​

Create 3 Topics for HW #2 reactor

$ ros2 topic list -t

/battery_state [sensor_msgs/msg/BatteryState]

/cmd_audio [irobot_create_msgs/msg/AudioNoteVector]

/cmd_lightring [irobot_create_msgs/msg/LightringLeds]

/cmd_vel [geometry_msgs/msg/Twist]

/dock [irobot_create_msgs/msg/Dock]

/hazard_detection [irobot_create_msgs/msg/HazardDetectionVector]

/imu [sensor_msgs/msg/Imu]

/interface_buttons [irobot_create_msgs/msg/InterfaceButtons]

/ir_intensity [irobot_create_msgs/msg/IrIntensityVector]

/ir_opcode [irobot_create_msgs/msg/IrOpcode]

/kidnap_status [irobot_create_msgs/msg/KidnapStatus]

/mouse [irobot_create_msgs/msg/Mouse]

/odom [nav_msgs/msg/Odometry]

/parameter_events [rcl_interfaces/msg/ParameterEvent]

/rosout [rcl_interfaces/msg/Log]

/slip_status [irobot_create_msgs/msg/SlipStatus]

/stop_status [irobot_create_msgs/msg/StopStatus]

/tf [tf2_msgs/msg/TFMessage]

/tf_static [tf2_msgs/msg/TFMessage]

/wheel_status [irobot_create_msgs/msg/WheelStatus]

/wheel_ticks [irobot_create_msgs/msg/WheelTicks]

/wheel_vels [irobot_create_msgs/msg/WheelVels]

Create 3 Topics for HW #2 reactor

IrIntensity message

IrIntensityVector message

← does not mean range! It's just the brightness of the reflected light

Possible Movement Strategies for reactor

• Basic actions: forward, forward-left, forward-right, stop & turn?
• Use all 7 terms individually? Or average them into "zones" -- e.g., left, center, right?
• What are the minimum conditions for forward motion?
• What would the robot need to "see" to cause it to stop completely?
• What would it need to see to cause a left/right turn?
• Should it slow down? Should there be gradual turns and hard turns?

On-Off Control

from Astrom & Murray

u = action, e = some error measurement from sensors

​

For example:

u = commanded angular velocity

e = difference between right & left zone average intensity

Control Considerations

• Error term e(t) computed at time t between the variable(s) of interest and their desired setpoint (aka target/goal/etc.)
• A control input u(t) is sought to drive the error to 0 and thus achieve the desired setpoint
• Issues
• Robustness to uncertainty
• Responsiveness: How quickly does the system approach the target?
• Oscillation: Does the system repeatedly overshoot the target?
• Deadband: How close is close enough to the setpoint that no control input is necessary or desirable?

PID Control

• Proportional control: u(t) = K_p e(t)
• Derivative: How is error changing?
• Dampens oscillations, but can be sensitive to noise
• Integral: How is error accumulating?
• Can overcome steady-state error, but exacerbate overshoot

diagram from Wikipedia

HW #2

• Friday: Release of UDBot robot description package for visualization

​

​

​