From e5dd1bb3ff30a86caf739642092d69b91d5ad9bb Mon Sep 17 00:00:00 2001 From: wittenator <9154515+wittenator@users.noreply.github.com> Date: Tue, 26 Dec 2023 18:17:41 +0100 Subject: [PATCH] Added working CAN and first draft of sim with chrono --- src/chabo_chrono | 1 + src/ft24_control/CMakeLists.txt | 48 +- src/ft24_control/config/ft24_config.yml | 2 +- .../description/real_robot_control.xacro | 4 - .../description/ros2_control.xacro | 4 - src/ft24_control/description/wheel.xacro | 8 + .../ft24_control/external_pos_setting.py | 84 ++ .../imu_covariance_adder.py | 0 src/ft24_control/{src => ft24_control}/joy.py | 0 src/ft24_control/ft24_control/testf1.py | 108 +++ .../{src => ft24_control}/twist.py | 0 .../include/SetLinkStatePlugin.hpp | 34 + src/ft24_control/src/SetLinkStatePlugin.cpp | 47 ++ src/ft24_control/src/__init__.py | 0 src/ft24_control/src/external_pose_set.cpp | 84 ++ src/ft24_control/src/testf1.py | 74 -- src/ft24_control/worlds/empty.sdf.template | 40 +- .../worlds/generate_track_world.py | 12 - .../worlds/generated_worlds/AU2_skidpad.sdf | 732 +----------------- .../ros2_control_can/ros2_control_can.h | 11 +- src/ros2_control_can/src/ros2_control_can.cpp | 106 ++- 21 files changed, 488 insertions(+), 911 deletions(-) create mode 160000 src/chabo_chrono create mode 100644 src/ft24_control/ft24_control/external_pos_setting.py rename src/ft24_control/{src => ft24_control}/imu_covariance_adder.py (100%) rename src/ft24_control/{src => ft24_control}/joy.py (100%) create mode 100644 src/ft24_control/ft24_control/testf1.py rename src/ft24_control/{src => ft24_control}/twist.py (100%) create mode 100644 src/ft24_control/include/SetLinkStatePlugin.hpp create mode 100644 src/ft24_control/src/SetLinkStatePlugin.cpp delete mode 100644 src/ft24_control/src/__init__.py create mode 100644 src/ft24_control/src/external_pose_set.cpp delete mode 100644 src/ft24_control/src/testf1.py diff --git a/src/chabo_chrono b/src/chabo_chrono new file mode 160000 index 0000000..3064103 --- /dev/null +++ b/src/chabo_chrono @@ -0,0 +1 @@ +Subproject commit 3064103811a228c1ad5e50f1b75006c04a36d6b9 diff --git a/src/ft24_control/CMakeLists.txt b/src/ft24_control/CMakeLists.txt index 87e2919..bdcc931 100644 --- a/src/ft24_control/CMakeLists.txt +++ b/src/ft24_control/CMakeLists.txt @@ -17,9 +17,43 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) +find_package(ignition-cmake2 REQUIRED) +find_package(ignition-msgs5 REQUIRED) +find_package(ignition-transport8 REQUIRED) +find_package(ignition-math6 REQUIRED) +find_package(ignition-common3 REQUIRED) + + +find_package(ignition-plugin1 REQUIRED COMPONENTS register) +set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR}) + +find_package(ignition-gazebo6 REQUIRED) +# Add sources for each plugin to be registered. +add_library(SetLinkStatePlugin src/SetLinkStatePlugin.cpp) + +target_include_directories( + SetLinkStatePlugin + PRIVATE + include +) + +set_property(TARGET SetLinkStatePlugin PROPERTY CXX_STANDARD 17) +target_link_libraries(SetLinkStatePlugin + PRIVATE ignition-plugin${IGN_PLUGIN_VER}::ignition-plugin${IGN_PLUGIN_VER} + PRIVATE ignition-gazebo6::ignition-gazebo6) + +add_executable(SetPos src/external_pose_set.cpp) + +target_include_directories( + SetPos + PRIVATE + include +) + +target_link_libraries(SetPos ${IGNITION-MSGS_LIBRARIES} ${IGNITION-TRANSPORT_LIBRARIES} ${IGNITION-COMMON_LIBRARIES} ${IGNITION-MATH_LIBRARIES}) + +install(TARGETS SetPos DESTINATION lib/${PROJECT_NAME}) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) @@ -33,7 +67,7 @@ if(BUILD_TESTING) endif() install( - DIRECTORY config description launch worlds src + DIRECTORY config description launch worlds ft24_control DESTINATION share/${PROJECT_NAME} ) @@ -41,9 +75,9 @@ ament_python_install_package(${PROJECT_NAME}) # Install Python executables install(PROGRAMS - src/imu_covariance_adder.py - src/joy.py - src/twist.py + ft24_control/imu_covariance_adder.py + ft24_control/joy.py + ft24_control/twist.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/src/ft24_control/config/ft24_config.yml b/src/ft24_control/config/ft24_config.yml index 49f8b55..da14094 100644 --- a/src/ft24_control/config/ft24_config.yml +++ b/src/ft24_control/config/ft24_config.yml @@ -21,7 +21,7 @@ ackermann_steering_controller: front_steering: true open_loop: false velocity_rolling_window_size: 10 - position_feedback: true + position_feedback: false use_stamped_vel: false rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint] front_wheels_names: [front_right_steer_joint, front_left_steer_joint] diff --git a/src/ft24_control/description/real_robot_control.xacro b/src/ft24_control/description/real_robot_control.xacro index be6abaa..eefff45 100644 --- a/src/ft24_control/description/real_robot_control.xacro +++ b/src/ft24_control/description/real_robot_control.xacro @@ -13,23 +13,19 @@ - - - - diff --git a/src/ft24_control/description/ros2_control.xacro b/src/ft24_control/description/ros2_control.xacro index 0678437..3dab1ac 100644 --- a/src/ft24_control/description/ros2_control.xacro +++ b/src/ft24_control/description/ros2_control.xacro @@ -6,23 +6,19 @@ - - - - diff --git a/src/ft24_control/description/wheel.xacro b/src/ft24_control/description/wheel.xacro index be0e2fa..8e7178f 100644 --- a/src/ft24_control/description/wheel.xacro +++ b/src/ft24_control/description/wheel.xacro @@ -322,6 +322,14 @@ Gazebo/Red + + + + 0 + 0 + + + \ No newline at end of file diff --git a/src/ft24_control/ft24_control/external_pos_setting.py b/src/ft24_control/ft24_control/external_pos_setting.py new file mode 100644 index 0000000..8f62770 --- /dev/null +++ b/src/ft24_control/ft24_control/external_pos_setting.py @@ -0,0 +1,84 @@ +#!/usr/bin/env python3 + +import math +import time + +# https://github.com/srmainwaring/python-ignition +from ignition.msgs.header_pb2 import Header +from ignition.msgs.pose_pb2 import Pose +from ignition.msgs.quaternion_pb2 import Quaternion +from ignition.msgs.vector3d_pb2 import Vector3d +from ignition.transport import Node + +# https://github.com/srmainwaring/python-ignition +import ignition.math + +# Settings +world_name = "empty" +model_name = "iris" + +timeout_ms = 10 +update_rate_hz = 30.0 + +# offsets - The ArduPilot z position may be zeroed to the ground plane, +# Gazebo will set pose for the base link geometric centre +pose_offset_x = 0.0 +pose_offset_y = 0.0 +pose_offset_z = 0.17 + + +def main(): + + connection_string = "udp:127.0.0.1:14550" + + # Connect to the Vehicle. + print("Connecting to vehicle on: {}".format(connection_string)) + + try: + # create a transport node + node = Node() + + # set service details + service = "/world/{}/set_pose".format(world_name) + reqtype = "ignition.msgs.Pose" + reptype = "ignition.msgs.Boolean" + + # configure update loop + now_s = time.time() + start_time_s = now_s + + # update_rate_hz = 1.0 + update_period_s = 1.0 / update_rate_hz + last_update_time_s = now_s + + sim_time_s = now_s - start_time_s + + while True: + now_s = time.time() + time_s = now_s - start_time_s + if now_s - last_update_time_s >= update_period_s: + last_update_time_s = now_s + + # check we have valid data + + # create request message + vector3d_msg = Vector3d() + vector3d_msg.x = 0.01 + pose_offset_x + vector3d_msg.y = 0.01+ pose_offset_y + vector3d_msg.z = pose_offset_z + + quat_msg = Quaternion() + + pose_msg = Pose() + pose_msg.name = model_name + pose_msg.position.CopyFrom(vector3d_msg) + pose_msg.orientation.CopyFrom(quat_msg) + + # submit request (blocking) + result = node.request(service, pose_msg, timeout_ms, reptype) + + if not result: + print("[{:.1f}] update failed".format(time_s)) + + except KeyboardInterrupt: + pass \ No newline at end of file diff --git a/src/ft24_control/src/imu_covariance_adder.py b/src/ft24_control/ft24_control/imu_covariance_adder.py similarity index 100% rename from src/ft24_control/src/imu_covariance_adder.py rename to src/ft24_control/ft24_control/imu_covariance_adder.py diff --git a/src/ft24_control/src/joy.py b/src/ft24_control/ft24_control/joy.py similarity index 100% rename from src/ft24_control/src/joy.py rename to src/ft24_control/ft24_control/joy.py diff --git a/src/ft24_control/ft24_control/testf1.py b/src/ft24_control/ft24_control/testf1.py new file mode 100644 index 0000000..8a322b4 --- /dev/null +++ b/src/ft24_control/ft24_control/testf1.py @@ -0,0 +1,108 @@ +import numpy as np +import cv2 +import matplotlib.pyplot as plt + +def generate_straight(length, width, start, direction=(1, 0)): + direction = np.array(direction) + norm_dir = direction / np.linalg.norm(direction) + left_edge = [start + norm_dir * width / 2 * np.array([-1, 1]) * i for i in range(length)] + right_edge = [start + norm_dir * width / 2 * np.array([1, -1]) * i for i in range(length)] + return left_edge, right_edge + +def generate_hairpin(radius, width, center, entry_angle=0): + angles = np.linspace(entry_angle, entry_angle + np.pi, 180) + left_edge = [center + (radius + width / 2) * np.array([np.cos(a), np.sin(a)]) for a in angles] + right_edge = [center + (radius - width / 2) * np.array([np.cos(a), np.sin(a)]) for a in angles] + return left_edge, right_edge + +def generate_chicane(segment_lengths, width, start, direction=(1, 0)): + direction = np.array(direction) + norm_dir = direction / np.linalg.norm(direction) + orth_dir = np.array([-norm_dir[1], norm_dir[0]]) + + points = [start] + for i, length in enumerate(segment_lengths): + start += norm_dir * length if i % 2 == 0 else orth_dir * width * ((i % 4) - 2) + points.append(start) + + left_edge = [p + orth_dir * width / 2 for p in points] + right_edge = [p - orth_dir * width / 2 for p in points] + + return left_edge, right_edge + +def generate_track(): + # Define the track dimensions and segments + straight_length = 100 + hairpin_radius = 20 + chicane_segment_lengths = [20, 10, 20] + track_width = 5 + + # Generate track segments + straight_start = np.array([0, 0]) + straight_end = np.array([straight_length, 0]) + left_straight, right_straight = generate_straight(straight_length, track_width, straight_start) + + hairpin_center = straight_end + np.array([0, -hairpin_radius]) + left_hairpin, right_hairpin = generate_hairpin(hairpin_radius, track_width, hairpin_center) + + chicane_start = left_hairpin[-1] + left_chicane, right_chicane = generate_chicane(chicane_segment_lengths, track_width, chicane_start) + + # Combine track segments + left_track = left_straight + left_hairpin[::-1] + left_chicane + right_track = right_straight + right_hairpin[::-1] + right_chicane + + # move center to (100, 100) + left_track = left_track + np.array([100, 100]) + right_track = right_track + np.array([100, 100]) + + return left_track, right_track + +def convert_to_grid_coordinates(cone, map_size_meters, map_resolution): + x, y = cone + grid_x = int(x / map_resolution) + grid_y = int(y / map_resolution) + return grid_x, grid_y + +def create_occupancy_grid(left_cones, right_cones, map_size_meters, map_resolution): + map_size_cells = int(map_size_meters / map_resolution) + grid_image = np.full((map_size_cells, map_size_cells), -1, dtype=np.int8) + + max_weight = 100 + min_weight = 50 + weight_decrement = 5 + cones = [] + + for cone in np.concatenate([left_cones, right_cones], axis=0): + left_x, left_y = convert_to_grid_coordinates(cone, map_size_meters, map_resolution) + + cones.append((left_x, left_y)) + cones = np.array([cones], dtype=np.int32) + cv2.fillPoly(grid_image, cones, color=0) + + return grid_image + +def plot_occupancy_grid(grid, map_size_meters, map_resolution): + plt.imshow(grid, cmap='gray', origin='lower') + plt.title('Occupancy Grid ({}m x {}m, {}m/pixel)'.format(map_size_meters, map_size_meters, map_resolution)) + plt.xlabel('X Position (m)') + plt.ylabel('Y Position (m)') + plt.xticks(np.arange(0, map_size_meters/map_resolution, step=10), np.arange(0, map_size_meters, step=10*map_resolution)) + plt.yticks(np.arange(0, map_size_meters/map_resolution, step=10), np.arange(0, map_size_meters, step=10*map_resolution)) + plt.show() + +# Test Parameters +map_size_meters = 200 +map_resolution = 0.1 # 1 meter per pixel for simplicity + +# Generate and plot test track +left_cones, right_cones = generate_track() +grid = create_occupancy_grid(left_cones, right_cones, map_size_meters, map_resolution) +plot_occupancy_grid(grid, map_size_meters, map_resolution) + + + + + + + diff --git a/src/ft24_control/src/twist.py b/src/ft24_control/ft24_control/twist.py similarity index 100% rename from src/ft24_control/src/twist.py rename to src/ft24_control/ft24_control/twist.py diff --git a/src/ft24_control/include/SetLinkStatePlugin.hpp b/src/ft24_control/include/SetLinkStatePlugin.hpp new file mode 100644 index 0000000..635cec7 --- /dev/null +++ b/src/ft24_control/include/SetLinkStatePlugin.hpp @@ -0,0 +1,34 @@ +#ifndef SET_LINK_STATE_SYSTEM_PLUGIN_H +#define SET_LINK_STATE_SYSTEM_PLUGIN_H + +#include +#include + + +using namespace gz::sim; + +namespace set_link_state_system_plugin +{ + class SetLinkStateSystemPlugin: + public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemUpdate + { + public: + /// \brief Constructor + SetLinkStateSystemPlugin() = default; + + /// \brief Destructor + virtual ~SetLinkStateSystemPlugin() = default; + + // Ignition Gazebo System interface + public: void Configure(const Entity & _entity, const std::shared_ptr & _sdf, + gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventManager) override; + + public: void Update(const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) override; + + }; +} + +#endif // SET_LINK_STATE_SYSTEM_PLUGIN_H diff --git a/src/ft24_control/src/SetLinkStatePlugin.cpp b/src/ft24_control/src/SetLinkStatePlugin.cpp new file mode 100644 index 0000000..87ff01a --- /dev/null +++ b/src/ft24_control/src/SetLinkStatePlugin.cpp @@ -0,0 +1,47 @@ +#include "../include/SetLinkStatePlugin.hpp" + +//! [registerSampleSystem] +#include +#include + +using namespace components; + +// Include a line in your source file for each interface implemented. +IGNITION_ADD_PLUGIN( + set_link_state_system_plugin::SetLinkStateSystemPlugin, + gz::sim::System, + set_link_state_system_plugin::SetLinkStateSystemPlugin::ISystemConfigure, + set_link_state_system_plugin::SetLinkStateSystemPlugin::ISystemUpdate +) +//! [registerSampleSystem] +//! [implementSampleSystem] +using namespace set_link_state_system_plugin; + +using namespace ignition::gazebo; + +// Ignition Gazebo System interface +void SetLinkStateSystemPlugin::Configure(const Entity & _entity, const std::shared_ptr & _sdf, + EntityComponentManager & _ecm, EventManager & _eventManager) +{ + + // Get the world entity + auto worldEntity = _ecm.EntityByComponents(components::Name("world"), components::World()); + if (worldEntity == kNullEntity) + { + ignerr << "Failed to find the world entity.\n"; + return; + } + + + + ignmsg << "SetLinkStateSystemPlugin configured.\n"; +} + +void SetLinkStateSystemPlugin::Update(const UpdateInfo & _info, EntityComponentManager & _ecm) +{ + // Get the current simulation time in Fortress + + // TODO: Implement communication with Fortress co-simulation script to get link states, velocities, accelerations, etc. + // +} + diff --git a/src/ft24_control/src/__init__.py b/src/ft24_control/src/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/src/ft24_control/src/external_pose_set.cpp b/src/ft24_control/src/external_pose_set.cpp new file mode 100644 index 0000000..7e74116 --- /dev/null +++ b/src/ft24_control/src/external_pose_set.cpp @@ -0,0 +1,84 @@ +#include +#include + +#include +#include +#include +#include + +// Settings +const std::string world_name = "empty"; +const std::string model_name = "spawn_robot"; + +const int timeout_ms = 10; +const double update_rate_hz = 100.0; + +// Offsets +double pose_offset_x = 0.0; +double pose_offset_y = 0.0; +double pose_offset_z = 0.17; + +int main() +{ + // Connect to the Ignition Transport node + ignition::transport::Node node; + + // Set service details + const std::string service = "/world/" + world_name + "/set_pose"; + const std::string reqtype = "ignition.msgs.Pose"; + const std::string reptype = "ignition.msgs.Boolean"; + + // Configure the update loop + auto start_time = std::chrono::steady_clock::now(); + double update_period_s = 1.0 / update_rate_hz; + auto last_update_time = start_time; + + while (true) + { + auto now = std::chrono::steady_clock::now(); + auto time_elapsed = std::chrono::duration_cast(now - start_time); + double time_s = time_elapsed.count() / 1000.0; + + if (std::chrono::duration_cast(now - last_update_time).count() >= update_period_s * 1000.0) + { + last_update_time = now; + + // Create request message + ignition::msgs::Vector3d vector3d_msg; + pose_offset_x = 10 * sin(time_s); + pose_offset_y = 10 * cos(time_s); + pose_offset_z = 0.17; + vector3d_msg.set_x(pose_offset_x); + vector3d_msg.set_y(pose_offset_y); + vector3d_msg.set_z(pose_offset_z); + + ignition::msgs::Quaternion quat_msg; + // follow circle + double yaw = atan2(pose_offset_y, pose_offset_x); + quat_msg.set_w(cos(yaw / 2)); + quat_msg.set_x(0); + quat_msg.set_y(0); + quat_msg.set_z(sin(yaw / 2)); + + ignition::msgs::Pose pose_msg; + pose_msg.set_name(model_name); + pose_msg.mutable_position()->CopyFrom(vector3d_msg); + pose_msg.mutable_orientation()->CopyFrom(quat_msg); + bool result; + ignition::msgs::Boolean result_msg; + + // Submit request (blocking) + result = node.Request(service, pose_msg, timeout_ms, result_msg, result); + + if (!result) + { + std::cout << "[" << time_s << "] Update failed" << std::endl; + } + } + + // Sleep for a short duration to avoid high CPU usage + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + return 0; +} diff --git a/src/ft24_control/src/testf1.py b/src/ft24_control/src/testf1.py deleted file mode 100644 index 2cd0ece..0000000 --- a/src/ft24_control/src/testf1.py +++ /dev/null @@ -1,74 +0,0 @@ -import cv2 -import numpy as np - -def draw_filled_track_boundaries(image, left_boundary, right_boundary): - # Combine left and right boundaries to create a single polygon - track_polygon = np.concatenate((left_boundary, right_boundary[::-1])) - - # Draw filled area between left and right boundaries - cv2.fillPoly(image, [track_polygon], color=(255, 255, 255)) - -def transform_boundaries(left_boundary, right_boundary, car_position, car_orientation): - # Homogeneous transformation matrix - transform_matrix = np.array([[np.cos(car_orientation), -np.sin(car_orientation), car_position[0]], - [np.sin(car_orientation), np.cos(car_orientation), car_position[1]], - [0, 0, 1]]) - - # Add homogeneous coordinate to boundaries - left_boundary_homogeneous = np.column_stack((left_boundary, np.ones(len(left_boundary)))) - right_boundary_homogeneous = np.column_stack((right_boundary, np.ones(len(right_boundary)))) - - # Apply transformation - left_boundary_transformed = np.dot(transform_matrix, left_boundary_homogeneous.T).T[:, :2].astype(np.int32) - right_boundary_transformed = np.dot(transform_matrix, right_boundary_homogeneous.T).T[:, :2].astype(np.int32) - - return left_boundary_transformed, right_boundary_transformed - -def main(): - # Image size and background color - image_size = (800, 600) - background_color = (0, 0, 0) # Black background - - # Create a black image - map_image = np.zeros((image_size[1], image_size[0], 3), dtype=np.uint8) - map_image[:] = background_color - - # Example curved track boundaries (replace with your own points) - num_points = 100 - theta = np.linspace(0, 2 * np.pi, num_points) - radius = 150 - - left_boundary_x = 400 + radius * np.cos(theta) - left_boundary_y = 300 + radius * np.sin(theta) - - right_boundary_x = 400 + radius * np.cos(theta - np.pi) - right_boundary_y = 500 + radius * np.sin(theta - np.pi) - - left_boundary = np.column_stack((left_boundary_x, left_boundary_y)).astype(np.int32) - right_boundary = np.column_stack((right_boundary_x, right_boundary_y)).astype(np.int32) - - # Car's global position and orientation (replace with your own values) - car_position = (100, 100) - car_orientation = np.pi / 4 # 45 degrees - - # Transform boundaries based on car's global position and orientation - left_boundary_transformed, right_boundary_transformed = transform_boundaries( - left_boundary, right_boundary, car_position, car_orientation - ) - - # Draw filled area between track boundaries on the image - draw_filled_track_boundaries(map_image, left_boundary_transformed, right_boundary_transformed) - - # Display the map image - cv2.imshow('Transformed Track Map', map_image) - cv2.waitKey(0) - cv2.destroyAllWindows() - -if __name__ == "__main__": - main() - - - - - - diff --git a/src/ft24_control/worlds/empty.sdf.template b/src/ft24_control/worlds/empty.sdf.template index c80842a..c9bf9e6 100644 --- a/src/ft24_control/worlds/empty.sdf.template +++ b/src/ft24_control/worlds/empty.sdf.template @@ -1,44 +1,24 @@ - + 0.005 - 1.0 - - - quick - true - true - cone_model - - - - - bullet - - + filename="ignition-gazebo-physics-system" + name="ignition::gazebo::systems::Physics"> + filename="ignition-gazebo-user-commands-system" + name="ignition::gazebo::systems::UserCommands"> + filename="ignition-gazebo-scene-broadcaster-system" + name="ignition::gazebo::systems::SceneBroadcaster"> - - - - - - ogre2 + + ogre2 false diff --git a/src/ft24_control/worlds/generate_track_world.py b/src/ft24_control/worlds/generate_track_world.py index eaf98d6..c0c8aac 100644 --- a/src/ft24_control/worlds/generate_track_world.py +++ b/src/ft24_control/worlds/generate_track_world.py @@ -185,19 +185,7 @@ if __name__ == "__main__": 255 - - - collision - - - - vehicle_blue - wall - - true - ''' cone_list_sdf.append(cone_sdf) diff --git a/src/ft24_control/worlds/generated_worlds/AU2_skidpad.sdf b/src/ft24_control/worlds/generated_worlds/AU2_skidpad.sdf index 6fab173..d67f5e1 100644 --- a/src/ft24_control/worlds/generated_worlds/AU2_skidpad.sdf +++ b/src/ft24_control/worlds/generated_worlds/AU2_skidpad.sdf @@ -1,44 +1,24 @@ - - + + 0.005 - 1.0 - - - quick - true - true - cone_model - - - - - bullet - - + name="ignition::gazebo::systems::Physics"> + name="ignition::gazebo::systems::UserCommands"> + name="ignition::gazebo::systems::SceneBroadcaster"> - - - - - - ogre2 + + ogre2 false @@ -105,19 +85,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -139,19 +107,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -173,19 +129,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -207,19 +151,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -241,19 +173,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -275,19 +195,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -309,19 +217,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -343,19 +239,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -377,19 +261,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -411,19 +283,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -445,19 +305,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -479,19 +327,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -513,19 +349,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -547,19 +371,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -581,19 +393,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -615,19 +415,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -649,19 +437,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -683,19 +459,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -717,19 +481,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -751,19 +503,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -785,19 +525,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -819,19 +547,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -853,19 +569,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -887,19 +591,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -921,19 +613,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -955,19 +635,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -989,19 +657,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -1023,19 +679,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -1057,19 +701,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -1091,19 +723,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -1125,19 +745,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -1159,19 +767,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -1193,19 +789,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -1227,19 +811,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -1261,19 +833,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -1295,19 +855,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -1329,19 +877,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -1363,19 +899,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -1397,19 +921,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -1431,19 +943,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -1465,19 +965,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -1499,19 +987,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -1533,19 +1009,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -1567,19 +1031,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -1601,19 +1053,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -1635,19 +1075,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -1669,19 +1097,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -1703,19 +1119,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -1737,19 +1141,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -1771,19 +1163,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -1805,19 +1185,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -1839,19 +1207,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -1873,19 +1229,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -1907,19 +1251,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -1941,19 +1273,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -1975,19 +1295,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -2009,19 +1317,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - @@ -2043,19 +1339,7 @@ 255 - - - collision - - - - vehicle_blue - wall - - true - diff --git a/src/ros2_control_can/include/ros2_control_can/ros2_control_can.h b/src/ros2_control_can/include/ros2_control_can/ros2_control_can.h index fc1b032..feba65c 100644 --- a/src/ros2_control_can/include/ros2_control_can/ros2_control_can.h +++ b/src/ros2_control_can/include/ros2_control_can/ros2_control_can.h @@ -14,6 +14,14 @@ #include "wheel.h" #include "can1__main_ft24.h" + +#include +#include +#include + +#include +#include + using hardware_interface::CallbackReturn; using hardware_interface::return_type; @@ -52,7 +60,8 @@ private: can1__main_ft24_jetson_tx_t tx_frame; can1__main_ft24_jetson_rx_t rx_frame; - uint8_t can_buffer[64]; + uint8_t* can_buffer; + struct can_frame frame; rclcpp::Logger logger_; diff --git a/src/ros2_control_can/src/ros2_control_can.cpp b/src/ros2_control_can/src/ros2_control_can.cpp index 405452e..508afdd 100644 --- a/src/ros2_control_can/src/ros2_control_can.cpp +++ b/src/ros2_control_can/src/ros2_control_can.cpp @@ -48,12 +48,41 @@ CallbackReturn Ros2ControlCAN::on_init(const hardware_interface::HardwareInfo & // Set up the wheels f_l_wheel_.setup(cfg_.front_left_wheel_name); f_r_wheel_.setup(cfg_.front_right_wheel_name); - r_l_wheel_.setup(cfg_.front_left_wheel_name); - r_r_wheel_.setup(cfg_.front_right_wheel_name); + r_l_wheel_.setup(cfg_.rear_left_wheel_name); + r_r_wheel_.setup(cfg_.rear_right_wheel_name); // init tx frame can1__main_ft24_jetson_tx_init(&tx_frame); + if ((socket_instance = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) { + perror("Socket"); + return CallbackReturn::ERROR; + } + + struct ifreq ifr; + const char *cstr = cfg_.device.c_str(); + strcpy(ifr.ifr_name, cstr ); + ioctl(socket_instance, SIOCGIFINDEX, &ifr); + + struct sockaddr_can addr; + memset(&addr, 0, sizeof(addr)); + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + if (bind(socket_instance, (struct sockaddr *)&addr, sizeof(addr)) < 0) { + perror("Bind"); + return CallbackReturn::ERROR; + } + + //struct can_filter rfilter[1]; + //rfilter[0].can_id = 0x550; + //rfilter[0].can_mask = 0xFF0; + //rfilter[1].can_id = 0x200; + //rfilter[1].can_mask = 0x700; + //setsockopt(socket_instance, SOL_CAN_RAW, CAN_RAW_FILTER, &rfilter, sizeof(rfilter)); + + // allocate can_buffer to 64 bit + can_buffer = (uint8_t*) malloc(8); + RCLCPP_INFO(logger_, "Finished Configuration"); return CallbackReturn::SUCCESS; @@ -69,8 +98,6 @@ std::vector Ros2ControlCAN::export_state_int state_interfaces.emplace_back(hardware_interface::StateInterface(f_r_wheel_.name, hardware_interface::HW_IF_POSITION, &f_r_wheel_.steer)); state_interfaces.emplace_back(hardware_interface::StateInterface(r_l_wheel_.name, hardware_interface::HW_IF_VELOCITY, &r_l_wheel_.vel)); state_interfaces.emplace_back(hardware_interface::StateInterface(r_r_wheel_.name, hardware_interface::HW_IF_VELOCITY, &r_r_wheel_.vel)); - state_interfaces.emplace_back(hardware_interface::StateInterface(r_l_wheel_.name, hardware_interface::HW_IF_POSITION, &r_l_wheel_.pos)); - state_interfaces.emplace_back(hardware_interface::StateInterface(r_r_wheel_.name, hardware_interface::HW_IF_POSITION, &r_r_wheel_.pos)); return state_interfaces; } @@ -93,34 +120,8 @@ CallbackReturn Ros2ControlCAN::on_activate(const rclcpp_lifecycle::State & previ { RCLCPP_INFO(logger_, "Starting Controller..."); - if ((socket_instance = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) { - perror("Socket"); - return CallbackReturn::ERROR; - } - - struct ifreq ifr; - const char *cstr = cfg_.device.c_str(); - strcpy(ifr.ifr_name, cstr ); - ioctl(socket_instance, SIOCGIFINDEX, &ifr); - - struct sockaddr_can addr; - memset(&addr, 0, sizeof(addr)); - addr.can_family = AF_CAN; - addr.can_ifindex = ifr.ifr_ifindex; - if (bind(socket_instance, (struct sockaddr *)&addr, sizeof(addr)) < 0) { - perror("Bind"); - return CallbackReturn::ERROR; - } - - struct can_filter rfilter[1]; - rfilter[0].can_id = 0x550; - rfilter[0].can_mask = 0xFF0; - //rfilter[1].can_id = 0x200; - //rfilter[1].can_mask = 0x700; - setsockopt(socket_instance, SOL_CAN_RAW, CAN_RAW_FILTER, &rfilter, sizeof(rfilter)); - - return CallbackReturn::SUCCESS; - } + return CallbackReturn::SUCCESS; +} CallbackReturn Ros2ControlCAN::on_deactivate(const rclcpp_lifecycle::State & previous_state) { @@ -145,49 +146,46 @@ return_type Ros2ControlCAN::read(const rclcpp::Time & /* time */, const rclcpp:: double deltaSeconds = diff.count(); time_ = new_time; - - if (false) - { + // read can frame + if (::read(socket_instance, &frame, sizeof(struct can_frame)) < 0) { + perror("Read from device: "); return return_type::ERROR; } - // convert rpm to rad/s - //l_wheel_.vel = RPMSTORADS(l_wheel_.vel); - //r_wheel_.vel = RPMSTORADS(r_wheel_.vel); + // decode can frame + int rx_size = can1__main_ft24_jetson_rx_unpack(&rx_frame, reinterpret_cast(&frame.data), sizeof(frame.data)); + + if (rx_size < 0) { + perror("Unpack"); + return return_type::ERROR; + } return return_type::OK; } -return_type Ros2ControlCAN::write(const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */) +return_type Ros2ControlCAN::write(const rclcpp::Time&, const rclcpp::Duration&) { - if (false) - { - return return_type::ERROR; - } + tx_frame.jetson_steering_angle = can1__main_ft24_jetson_tx_jetson_steering_angle_encode((f_l_wheel_.steer + f_r_wheel_.steer) / 2.0); + tx_frame.jetson_speed_target = can1__main_ft24_jetson_tx_jetson_speed_target_encode((r_l_wheel_.vel + r_r_wheel_.vel) / 2.0); - // send can frame - tx_frame.jetson_steering_angle = can1__main_ft24_jetson_tx_jetson_steering_angle_encode((f_l_wheel_.steer + f_r_wheel_.steer)/2.0); - - // pack tx_frame - int tx_size = can1__main_ft24_jetson_tx_pack(can_buffer, &tx_frame, 64); + int tx_size = can1__main_ft24_jetson_tx_pack(reinterpret_cast(&frame.data), &tx_frame, sizeof(frame.data)); if (tx_size < 0) { perror("Pack"); return return_type::ERROR; } - // send tx_frame - struct can_frame frame; - frame.can_id = 0x550; + frame.can_id = 0xe1; frame.can_dlc = tx_size; - memcpy(frame.data, can_buffer, tx_size); + if (::write(socket_instance, &frame, sizeof(struct can_frame)) < 0) { - perror("Write"); + perror("Write to device: "); + // print error with socket instance info return return_type::ERROR; } - return return_type::OK; + return return_type::OK; } } // namespace ros2_control_can