Click ik

CMakeLists.txt

@@ -67,15 +67,19 @@ set(MROVER_ROS_DEPENDENCIES
         tf2
         tf2_ros
         tf2_geometry_msgs
+        actionlib_msgs
 )
 
 extract_filenames(msg/*.msg MROVER_MESSAGE_FILES)
 
 extract_filenames(srv/*.srv MROVER_SERVICE_FILES)
 
+extract_filenames(action/*.action MROVER_ACTION_FILES)
+
 set(MROVER_MESSAGE_DEPENDENCIES
         std_msgs
         sensor_msgs
+        actionlib_msgs
 )
 
 set(MROVER_PARAMETERS
@@ -115,14 +119,16 @@ endforeach ()
 
 set(BUILD_SHARED_LIBS ON)
 
-### ======== ###
-### Messages ###
-### ======== ###
+### ================================ ###
+### Messages & Actions & Reconfigure ###
+### ================================ ###
 
 add_message_files(FILES ${MROVER_MESSAGE_FILES})
 
 add_service_files(FILES ${MROVER_SERVICE_FILES})
 
+add_action_files(DIRECTORY action FILES ${MROVER_ACTION_FILES})
+
 generate_messages(DEPENDENCIES ${MROVER_MESSAGE_DEPENDENCIES})
 
 generate_dynamic_reconfigure_options(${MROVER_PARAMETERS})
@@ -191,6 +197,9 @@ mrover_nodelet_link_libraries(long_range_tag_detector opencv_core opencv_objdete
 mrover_add_nodelet(usb_camera src/perception/usb_camera/*.cpp src/perception/usb_camera src/perception/usb_camera/pch.hpp)
 mrover_nodelet_link_libraries(usb_camera opencv_core opencv_objdetect opencv_aruco opencv_imgproc opencv_highgui)
 
+mrover_add_nodelet(click_ik src/perception/click_ik/*.cpp src/perception/click_ik src/perception/click_ik/pch.hpp)
+mrover_nodelet_link_libraries(click_ik lie)
+
 if (CUDA_FOUND)
     #    mrover_add_node(nv_vid_codec_h265_enc src/esw/nv_vid_codec_h265_enc/*.c*)
     #    target_link_libraries(nv_vid_codec_h265_enc PUBLIC cuda nvidia-encode opencv_core opencv_imgproc streaming)

action/ClickIk.action

@@ -0,0 +1,6 @@
+uint32 pointInImageX
+uint32 pointInImageY
+---
+bool success
+---
+float32 distance

launch/simulator.launch

@@ -1,26 +1,33 @@
 <launch>
-  <arg name="headless" default="false"/>
-  <arg name="run_rviz" default="true"/>
+  <arg name="headless" default="false" />
+  <arg name="run_rviz" default="true" />
 
-  <rosparam command="load" file="$(find mrover)/config/simulator/simulator.yaml"/>
+  <rosparam command="load" file="$(find mrover)/config/simulator/simulator.yaml" />
 
   <include file="$(find mrover)/launch/autonomy.launch">
-    <arg name="sim" value="true"/>
-    <arg name="ekf_start_delay" value="1"/>
+    <arg name="sim" value="true" />
+    <arg name="ekf_start_delay" value="1" />
   </include>
 
   <node pkg="nodelet" type="nodelet" name="perception_nodelet_manager" required="true"
-        args="manager" output="screen"/>
+    args="manager" output="screen" />
 
   <node pkg="nodelet" type="nodelet" name="simulator_nodelet"
-        args="load mrover/SimulatorNodelet perception_nodelet_manager" output="screen">
-    <param name="headless" value="$(arg headless)"/>
+    args="load mrover/SimulatorNodelet perception_nodelet_manager" output="screen">
+    <param name="headless" value="$(arg headless)" />
   </node>
 
-  <node name="arm_controller" pkg="mrover" type="arm_controller" output="screen"/>
+
+  <!-- Transform Values Taken from URDF 3/10/24-->
+  <!-- <node pkg="tf" type="static_transform_publisher" name="joint_a_static_broadcaster"
+    args="0.164882 0.200235 0.491172 0 0 0 base_link joint_a_static 10" />
+  <node pkg="tf" type="static_transform_publisher" name="joint_b_static_broadcaster"
+    args="0.034346 0.0 0.049024 0 0 0 joint_a_static joint_b_static 10" /> -->
+
+  <node name="arm_controller" pkg="mrover" type="arm_controller" output="screen" />
 
   <group if="$(arg run_rviz)">
-    <arg name="rvizconfig" default="$(find mrover)/config/rviz/auton_sim.rviz"/>
-    <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)"/>
+    <arg name="rvizconfig" default="$(find mrover)/config/rviz/auton_sim.rviz" />
+    <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" />
   </group>
-</launch>
+</launch>

msg/ArmStatus.msg

@@ -0,0 +1 @@
+bool status

package.xml

@@ -85,5 +85,6 @@
     <nodelet plugin="${prefix}/plugins/can.xml"/>
     <nodelet plugin="${prefix}/plugins/simulator.xml"/>
     <nodelet plugin="${prefix}/plugins/gst_websocket_streamer.xml"/>
+    <nodelet plugin="${prefix}/plugins/click_ik.xml" />
   </export>
 </package>

plugins/click_ik.xml

@@ -0,0 +1,6 @@
+<library path="lib/libclick_ik_nodelet">
+    <class name="mrover/ClickIkNodelet"
+           type="mrover::ClickIkNodelet"
+           base_class_type="nodelet::Nodelet">
+    </class>
+</library>

scripts/debug_click_ik.py

@@ -0,0 +1,40 @@
+#!/usr/bin/env python3
+
+import rospy
+from mrover.msg import ClickIkAction, ClickIkGoal
+import actionlib
+import sys
+
+
+def click_ik_client():
+    # Creates the SimpleActionClient, passing the type of the action
+    client = actionlib.SimpleActionClient("do_click_ik", ClickIkAction)
+    print("created click ik client")
+    # Waits until the action server has started up and started
+    # listening for goals.
+    client.wait_for_server()
+    print("Found server")
+    # Creates a goal to send to the action server.
+    goal = ClickIkGoal()
+    # Sends the goal to the action server.
+    goal.pointInImageX = 320
+    goal.pointInImageY = 240
+    client.send_goal(goal, feedback_cb=feedback)
+    client.wait_for_result()
+    result = client.get_result()
+    return result
+
+
+def feedback(msg):
+    print(msg)
+
+
+if __name__ == "__main__":
+    try:
+        # Initializes a rospy node so that the SimpleActionClient can
+        # publish and subscribe over ROS.
+        rospy.init_node("debug_click_ik")
+        result = click_ik_client()
+        print("result: ", result)
+    except rospy.ROSInterruptException:
+        print("program interrupted before completion", file=sys.stderr)

scripts/debug_service.py

@@ -6,16 +6,16 @@
 
 from typing import Any
 import rospy
-from std_srvs.srv import SetBool, SetBoolResponse
+from mrover.srv import ChangeCameras, ChangeCamerasResponse
 
 # Change these values for the service name and type definition to test different values
-SERVICE_NAME = "science_enable_heater_b1"
-SERVICE_TYPE = SetBool
+SERVICE_NAME = "change_cameras"
+SERVICE_TYPE = ChangeCameras
 
 
 def print_service_request(service_request: Any):
     rospy.loginfo(service_request)
-    return SetBoolResponse(success=True)
+    return ChangeCamerasResponse(success=True)
 
 
 def main():

src/esw/fw/pdlb_bare/CMakeLists.txt

@@ -0,0 +1,73 @@
+#THIS FILE IS AUTO GENERATED FROM THE TEMPLATE! DO NOT CHANGE!
+set(CMAKE_SYSTEM_NAME Generic)
+set(CMAKE_SYSTEM_VERSION 1)
+cmake_minimum_required(VERSION 3.28)
+
+# specify cross-compilers and tools
+set(CMAKE_C_COMPILER arm-none-eabi-gcc)
+set(CMAKE_CXX_COMPILER arm-none-eabi-g++)
+set(CMAKE_ASM_COMPILER  arm-none-eabi-gcc)
+set(CMAKE_AR arm-none-eabi-ar)
+set(CMAKE_OBJCOPY arm-none-eabi-objcopy)
+set(CMAKE_OBJDUMP arm-none-eabi-objdump)
+set(SIZE arm-none-eabi-size)
+set(CMAKE_TRY_COMPILE_TARGET_TYPE STATIC_LIBRARY)
+
+# project settings
+project(pdlb_bare C CXX ASM)
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_C_STANDARD 11)
+
+#Uncomment for hardware floating point
+#add_compile_definitions(ARM_MATH_CM4;ARM_MATH_MATRIX_CHECK;ARM_MATH_ROUNDING)
+#add_compile_options(-mfloat-abi=hard -mfpu=fpv4-sp-d16)
+#add_link_options(-mfloat-abi=hard -mfpu=fpv4-sp-d16)
+
+#Uncomment for software floating point
+#add_compile_options(-mfloat-abi=soft)
+
+add_compile_options(-mcpu=cortex-m4 -mthumb -mthumb-interwork)
+add_compile_options(-ffunction-sections -fdata-sections -fno-common -fmessage-length=0)
+
+# uncomment to mitigate c++17 absolute addresses warnings
+#set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-register")
+
+# Enable assembler files preprocessing
+add_compile_options($<$<COMPILE_LANGUAGE:ASM>:-x$<SEMICOLON>assembler-with-cpp>)
+
+if ("${CMAKE_BUILD_TYPE}" STREQUAL "Release")
+    message(STATUS "Maximum optimization for speed")
+    add_compile_options(-Ofast)
+elseif ("${CMAKE_BUILD_TYPE}" STREQUAL "RelWithDebInfo")
+    message(STATUS "Maximum optimization for speed, debug info included")
+    add_compile_options(-Ofast -g)
+elseif ("${CMAKE_BUILD_TYPE}" STREQUAL "MinSizeRel")
+    message(STATUS "Maximum optimization for size")
+    add_compile_options(-Os)
+else ()
+    message(STATUS "Minimal optimization, debug info included")
+    add_compile_options(-Og -g)
+endif ()
+
+include_directories(Core/Inc Drivers/STM32G4xx_HAL_Driver/Inc Drivers/STM32G4xx_HAL_Driver/Inc/Legacy Drivers/CMSIS/Device/ST/STM32G4xx/Include Drivers/CMSIS/Include)
+
+add_definitions(-DDEBUG -DUSE_HAL_DRIVER -DSTM32G431xx)
+
+file(GLOB_RECURSE SOURCES "Core/*.*" "Drivers/*.*")
+
+set(LINKER_SCRIPT ${CMAKE_SOURCE_DIR}/)
+
+add_link_options(-Wl,-gc-sections,--print-memory-usage,-Map=${PROJECT_BINARY_DIR}/${PROJECT_NAME}.map)
+add_link_options(-mcpu=cortex-m4 -mthumb -mthumb-interwork)
+add_link_options(-T ${LINKER_SCRIPT})
+
+add_executable(${PROJECT_NAME}.elf ${SOURCES} ${LINKER_SCRIPT})
+
+set(HEX_FILE ${PROJECT_BINARY_DIR}/${PROJECT_NAME}.hex)
+set(BIN_FILE ${PROJECT_BINARY_DIR}/${PROJECT_NAME}.bin)
+
+add_custom_command(TARGET ${PROJECT_NAME}.elf POST_BUILD
+        COMMAND ${CMAKE_OBJCOPY} -Oihex $<TARGET_FILE:${PROJECT_NAME}.elf> ${HEX_FILE}
+        COMMAND ${CMAKE_OBJCOPY} -Obinary $<TARGET_FILE:${PROJECT_NAME}.elf> ${BIN_FILE}
+        COMMENT "Building ${HEX_FILE}
+Building ${BIN_FILE}")

src/esw/fw/pdlb_bare/CMakeLists_template.txt

@@ -0,0 +1,72 @@
+#${templateWarning}
+set(CMAKE_SYSTEM_NAME Generic)
+set(CMAKE_SYSTEM_VERSION 1)
+${cmakeRequiredVersion}
+# specify cross-compilers and tools
+set(CMAKE_C_COMPILER arm-none-eabi-gcc)
+set(CMAKE_CXX_COMPILER arm-none-eabi-g++)
+set(CMAKE_ASM_COMPILER  arm-none-eabi-gcc)
+set(CMAKE_AR arm-none-eabi-ar)
+set(CMAKE_OBJCOPY arm-none-eabi-objcopy)
+set(CMAKE_OBJDUMP arm-none-eabi-objdump)
+set(SIZE arm-none-eabi-size)
+set(CMAKE_TRY_COMPILE_TARGET_TYPE STATIC_LIBRARY)
+
+# project settings
+project(${projectName} C CXX ASM)
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_C_STANDARD 11)
+
+#Uncomment for hardware floating point
+#add_compile_definitions(ARM_MATH_CM4;ARM_MATH_MATRIX_CHECK;ARM_MATH_ROUNDING)
+#add_compile_options(-mfloat-abi=hard -mfpu=fpv4-sp-d16)
+#add_link_options(-mfloat-abi=hard -mfpu=fpv4-sp-d16)
+
+#Uncomment for software floating point
+#add_compile_options(-mfloat-abi=soft)
+
+add_compile_options(-mcpu=${mcpu} -mthumb -mthumb-interwork)
+add_compile_options(-ffunction-sections -fdata-sections -fno-common -fmessage-length=0)
+
+# uncomment to mitigate c++17 absolute addresses warnings
+#set(CMAKE_CXX_FLAGS "$${CMAKE_CXX_FLAGS} -Wno-register")
+
+# Enable assembler files preprocessing
+add_compile_options($<$<COMPILE_LANGUAGE:ASM>:-x$<SEMICOLON>assembler-with-cpp>)
+
+if ("$${CMAKE_BUILD_TYPE}" STREQUAL "Release")
+    message(STATUS "Maximum optimization for speed")
+    add_compile_options(-Ofast)
+elseif ("$${CMAKE_BUILD_TYPE}" STREQUAL "RelWithDebInfo")
+    message(STATUS "Maximum optimization for speed, debug info included")
+    add_compile_options(-Ofast -g)
+elseif ("$${CMAKE_BUILD_TYPE}" STREQUAL "MinSizeRel")
+    message(STATUS "Maximum optimization for size")
+    add_compile_options(-Os)
+else ()
+    message(STATUS "Minimal optimization, debug info included")
+    add_compile_options(-Og -g)
+endif ()
+
+include_directories(${includes})
+
+add_definitions(${defines})
+
+file(GLOB_RECURSE SOURCES ${sources})
+
+set(LINKER_SCRIPT $${CMAKE_SOURCE_DIR}/${linkerScript})
+
+add_link_options(-Wl,-gc-sections,--print-memory-usage,-Map=$${PROJECT_BINARY_DIR}/$${PROJECT_NAME}.map)
+add_link_options(-mcpu=${mcpu} -mthumb -mthumb-interwork)
+add_link_options(-T $${LINKER_SCRIPT})
+
+add_executable($${PROJECT_NAME}.elf $${SOURCES} $${LINKER_SCRIPT})
+
+set(HEX_FILE $${PROJECT_BINARY_DIR}/$${PROJECT_NAME}.hex)
+set(BIN_FILE $${PROJECT_BINARY_DIR}/$${PROJECT_NAME}.bin)
+
+add_custom_command(TARGET $${PROJECT_NAME}.elf POST_BUILD
+        COMMAND $${CMAKE_OBJCOPY} -Oihex $<TARGET_FILE:$${PROJECT_NAME}.elf> $${HEX_FILE}
+        COMMAND $${CMAKE_OBJCOPY} -Obinary $<TARGET_FILE:$${PROJECT_NAME}.elf> $${BIN_FILE}
+        COMMENT "Building $${HEX_FILE}
+Building $${BIN_FILE}")

src/esw/fw/pdlb_bare/st_nucleo_g4.cfg

@@ -0,0 +1,19 @@
+# SPDX-License-Identifier: GPL-2.0-or-later
+
+# This is for all ST NUCLEO with any STM32G4. Known boards at the moment:
+# NUCLEO-G431KB
+# https://www.st.com/en/evaluation-tools/nucleo-g431kb.html
+# NUCLEO-G431RB
+# https://www.st.com/en/evaluation-tools/nucleo-g431rb.html
+# NUCLEO-G474RE
+# https://www.st.com/en/evaluation-tools/nucleo-g474re.html
+# NUCLEO-G491RE
+# https://www.st.com/en/evaluation-tools/nucleo-g491re.html
+
+source [find interface/stlink.cfg]
+
+transport select hla_swd
+
+source [find target/stm32g4x.cfg]
+
+reset_config srst_only